add sensor fusion function -----liusunan...

autogen.sh

@@ -1,7 +1,7 @@
 
-qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
+#qtmake="/opt/Qt5.10.1/5.10.1/gcc_64/bin/qmake"
 #qtmake=/usr/bin/qmake
-#qtmake="/usr/lib/aarch64-linux-gnu/qt5/bin/qmake"
+qtmake="/usr/lib/aarch64-linux-gnu/qt5/bin/qmake"
 if [ ! $qtmake ];then
 	echo -e "\e[33m qtmake not set, auto find it\e[0m"
 	qtmake=`find /opt -name "qmake" 2>/dev/null | grep 'gcc_64'`
@@ -134,6 +134,8 @@ rm Makefile
 rm .qmake.stash
 cd ../../../
 
+
+
 controller_app_name=(
 	controller_ge3
 	controller_vv7

src/decition/common/common/boost.h

@@ -1,51 +1,51 @@
-#pragma once
-
-#ifndef _IV_BOOST_H_
-#define _IV_BOOST_H_
-
-#if defined __GNUC__
-#  pragma GCC system_header 
-#endif
-#ifndef __CUDACC__
-//https://bugreports.qt-project.org/browse/QTBUG-22829
-#ifndef Q_MOC_RUN
-#include <boost/version.hpp>
-#include <boost/numeric/conversion/cast.hpp>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/condition.hpp>
-#include <boost/thread.hpp>
-#include <boost/thread/thread.hpp>
-#include <boost/filesystem.hpp>
-#include <boost/bind.hpp>
-#include <boost/cstdint.hpp>
-#include <boost/function.hpp>
-#include <boost/tuple/tuple.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include <boost/mpl/fold.hpp>
-#include <boost/mpl/inherit.hpp>
-#include <boost/mpl/inherit_linearly.hpp>
-#include <boost/mpl/joint_view.hpp>
-#include <boost/mpl/transform.hpp>
-#include <boost/mpl/vector.hpp>
-#include <boost/algorithm/string.hpp>
-#ifndef Q_MOC_RUN
-#include <boost/date_time/posix_time/posix_time.hpp>
-#endif
-#if BOOST_VERSION >= 104700
-#include <boost/chrono.hpp>
-#endif
-#include <boost/tokenizer.hpp>
-#include <boost/foreach.hpp>
-#include <boost/shared_array.hpp>
-#include <boost/interprocess/sync/file_lock.hpp>
-#if BOOST_VERSION >= 104900
-#include <boost/interprocess/permissions.hpp>
-#endif
-#include <boost/iostreams/device/mapped_file.hpp>
-#define BOOST_PARAMETER_MAX_ARITY 7
-#include <boost/signals2.hpp>
-#include <boost/signals2/slot.hpp>
-#endif
-#endif
-#endif    // _IV_BOOST_H_
+#pragma once
+
+#ifndef _IV_BOOST_H_
+#define _IV_BOOST_H_
+
+#if defined __GNUC__
+#  pragma GCC system_header 
+#endif
+#ifndef __CUDACC__
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/bind.hpp>
+#include <boost/cstdint.hpp>
+#include <boost/function.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/mpl/fold.hpp>
+#include <boost/mpl/inherit.hpp>
+#include <boost/mpl/inherit_linearly.hpp>
+#include <boost/mpl/joint_view.hpp>
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/vector.hpp>
+#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
+#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
+#include <boost/chrono.hpp>
+#endif
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/shared_array.hpp>
+#include <boost/interprocess/sync/file_lock.hpp>
+#if BOOST_VERSION >= 104900
+#include <boost/interprocess/permissions.hpp>
+#endif
+#include <boost/iostreams/device/mapped_file.hpp>
+#define BOOST_PARAMETER_MAX_ARITY 7
+#include <boost/signals2.hpp>
+#include <boost/signals2/slot.hpp>
+#endif
+#endif
+#endif    // _IV_BOOST_H_

src/decition/common/common/car_status.cpp

@@ -1,24 +1,24 @@
-#include "car_status.h"
-
-#ifdef linux
-
-unsigned long GetTickCount()
-{
- struct timespec ts;
- clock_gettime(CLOCK_MONOTONIC,&ts);
- return (ts.tv_sec * 1000 + ts.tv_nsec/(1000*1000) );
-}
-#endif
-
-iv::CarStatus::CarStatus()
-{
-	speed = 0;
-	braking_pressure = 0;
-	wheel_angle = 0;
-	location = boost::shared_ptr<iv::GPS_INS>(new iv::GPS_INS);
-    mRunTime.start();
-}
-
-iv::CarStatus::~CarStatus()
-{
-}
+#include "car_status.h"
+
+#ifdef linux
+
+unsigned long GetTickCount()
+{
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC,&ts);
+ return (ts.tv_sec * 1000 + ts.tv_nsec/(1000*1000) );
+}
+#endif
+
+iv::CarStatus::CarStatus()
+{
+	speed = 0;
+	braking_pressure = 0;
+	wheel_angle = 0;
+	location = boost::shared_ptr<iv::GPS_INS>(new iv::GPS_INS);
+    mRunTime.start();
+}
+
+iv::CarStatus::~CarStatus()
+{
+}

src/decition/common/common/car_status.h

@@ -1,188 +1,188 @@
-#pragma once
-#ifndef _IV_COMMON_CAR_STATUS_
-#define _IV_COMMON_CAR_STATUS_
-
-#include <common/boost.h>
-#include <boost/serialization/singleton.hpp>
-#include <common/gps_type.h>
-#include <common/obstacle_type.h>
-#include <time.h>
-#include <control/mobileye.h>
-#include <QTime>
-
-///kkcai, 2020-01-03
-#include <QMutex>
-///////////////////////////////////////
-
-#include "platform/platform.h"
-
-#include "ultrasonic_type.h"
-
-//#include "common/decitionview.pb.h"
-
-#include "common/sysparam_type.h"
-
-#include "common/roadmode_type.h"
-
-#define RADAR_OBJ_NUM 64
-
-#ifdef linux
-unsigned long GetTickCount();
-#endif
-namespace iv {
-    class CarStatus : public boost::noncopyable {
-    public:
-        CarStatus();
-        ~CarStatus();
-
-        float speed;			//车速
-        double mfCalAcc = 0;
-        std::int16_t wheel_angle;		//方向盘转角
-        std::uint8_t braking_pressure;	//刹车压力
-         float torque_st=0;
-        bool mbRunPause = true;
-        bool mbBrainCtring = false;
-        bool status[6] = { true, false, false, false, true, false };	//bool arrive = false;	//  x4:是否到达站点（0:未到 1:到达）
-                                                                //bool people = false;	//	x3:车上是否有人（0:无人 1:有人）
-                                                                //bool stop = false;	    //	x2:是否停车（0:否   1:是）
-                                                                //bool call = false;		//	x1:是否叫车（0:否	1:是）
-                                                                //bool available = true;	// 是否可被叫车
-                                                                //bool fire = false;
- //       int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
-
-        int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
-                int istostation = 0;
-                //int ctostation = 0;
-                int currentstation = 0;
-                //int nextstation = 0;
-                int clientto = 0;
-                double amilng = 0.0;
-                double amilat = 0.0;
-                bool busmode = false;
-                bool netconnect = false;
-                bool doorsta=false;
-                unsigned char speedlimte;
-
-        int grade=0;
-        int driverMode=0;
-        int epb=0;
-        int epsMode=1;
-        bool receiveEps=false;
-
-
-              bool speed_control=true;
-              bool group_control =false;
-              int group_server_status=0;
-              int  group_state = 0;
-              float group_x_local = 0.0;
-              float group_y_local = 0.0;
-              float group_velx_local = 0.0;
-              float group_vely_local = 0.0;
-              float group_comm_speed = 0.0;
-              int group_pathpoint=0;
-              float group_offsetx=0.0;
-              float group_frontDistance=0.0;
-
-              float mfttc = 0;
-
-
-        std::vector <iv::platform::station> car_stations;
-
-
-        AWS_display aws_display;
-        lane Lane;
-        obstacle_status obstacleStatus;
-        obstacle_data_a obstacleStatusA[15];
-        obstacle_data_b obstacleStatusB[15];
-        obstacle_data_c obstacleStatusC[15];
-        aftermarket_lane aftermarketLane;
-        lka_left_lane_a LKAleftLaneA;
-        lka_left_lane_b LKAleftLaneB;
-        lka_right_lane_a LKArightLaneA;
-        lka_right_lane_b LKArightLaneB;
-        next_lane_a nextLaneA_left[4], nextLaneA_right[4];
-        next_lane_b nextLaneB_left[4], nextLaneB_right[4];
-        ref_points refPoints;
-        num_of_next_lane_mark_reported numOfNextLaneMarkReported;
-
-
-        double mfAcc = 0;
-        double mfVehAcc=0;
-        double mfWheel = 0;
-        double mfBrake = 0;
-        double steerAngle=0;
-        QTime mRunTime;
-
-        int mRadarS = -1;
-        int mLidarS = -1;
-        int mRTKStatus = 0;
-        int mLidarPer = -1;
-
-        double mLidarObs;
-        double mRadarObs;
-        double mObs;
-
-
-        GPS_INS aim_gps_ins;
-        bool bocheMode=false;
-        int bocheEnable=0;
-
-        double mfParkLat;
-        double mfParkLng;
-        double mfParkHeading;
-        int mnParkType;
-
-
-        double mLidarRotation;
-        double mLidarRangeUnit;
-
-        iv::GPSData location;		//当前车辆位置
-        boost::array<iv::ObstacleBasic, 64> obs_radar;//毫米波雷达的障碍物数据
-         boost::array<iv::ObstacleBasic, 64> obs_rear_radar;//houxiang毫米波雷达的障碍物数据
-        iv::ultrasonic_obs multrasonic_obs;
-        double mfGPSAcc = 0;
-
-//        iv::brain::decitionview mdecitionview;
-        iv::sysparam msysparam;
-        iv::roadmode_vel mroadmode_vel;
-        int vehGroupId;
-        double mfEPSOff = 0.0;
-        float socfDis=15;   //停障触发距离
-        float aocfDis=25;   //避障触发距离
-        float aocfTimes=5; //避障触发次数
-        float aobzDis=5;   //避障保障距离
-        /// traffice light : 0x90
-        int iTrafficeLightID = 0;       //红绿灯ID
-        int iTrafficeLightColor = 2;    //0x0: 红色
-        //0x1: 黄色
-        //0x2: 绿色
-        int iTrafficeLightTime = 0;     //红绿灯剩余时间
-        double iTrafficeLightLat = 0;
-        double iTrafficeLightLon = 0;
-
-        bool daocheMode=false;
-
-        //////////////////////////////////////////////////////
-
-        ///kkcai, 2020-01-03
-        QMutex mMutexMap;
-        /////////////////////////////////////
-
-        //mobileEye chuku
-
-        float vehSpeed_st=0;
-
-        bool useMobileEye = false;
-        bool m_bOutGarage=false;//车道线识别出库标志
-        float outGarageLong=10;
-        float waitGpsTimeWidth=5000;
-
-        int mnBocheComplete = 0;   //When boche comple set a value.
-
-        bool useObsPredict = false;
-
-    };
-    typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
-}
-#define ServiceCarStatus iv::CarStatusSingleton::get_mutable_instance()
-#endif // !_IV_COMMON_CAR_STATUS_
+#pragma once
+#ifndef _IV_COMMON_CAR_STATUS_
+#define _IV_COMMON_CAR_STATUS_
+
+#include <common/boost.h>
+#include <boost/serialization/singleton.hpp>
+#include <common/gps_type.h>
+#include <common/obstacle_type.h>
+#include <time.h>
+#include <control/mobileye.h>
+#include <QTime>
+
+///kkcai, 2020-01-03
+#include <QMutex>
+///////////////////////////////////////
+
+#include "platform/platform.h"
+
+#include "ultrasonic_type.h"
+
+//#include "common/decitionview.pb.h"
+
+#include "common/sysparam_type.h"
+
+#include "common/roadmode_type.h"
+
+#define RADAR_OBJ_NUM 64
+
+#ifdef linux
+unsigned long GetTickCount();
+#endif
+namespace iv {
+    class CarStatus : public boost::noncopyable {
+    public:
+        CarStatus();
+        ~CarStatus();
+
+        float speed;			//车速
+        double mfCalAcc = 0;
+        std::int16_t wheel_angle;		//方向盘转角
+        std::uint8_t braking_pressure;	//刹车压力
+         float torque_st=0;
+        bool mbRunPause = true;
+        bool mbBrainCtring = false;
+        bool status[6] = { true, false, false, false, true, false };	//bool arrive = false;	//  x4:是否到达站点（0:未到 1:到达）
+                                                                //bool people = false;	//	x3:车上是否有人（0:无人 1:有人）
+                                                                //bool stop = false;	    //	x2:是否停车（0:否   1:是）
+                                                                //bool call = false;		//	x1:是否叫车（0:否	1:是）
+                                                                //bool available = true;	// 是否可被叫车
+                                                                //bool fire = false;
+ //       int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
+
+        int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
+                int istostation = 0;
+                //int ctostation = 0;
+                int currentstation = 0;
+                //int nextstation = 0;
+                int clientto = 0;
+                double amilng = 0.0;
+                double amilat = 0.0;
+                bool busmode = false;
+                bool netconnect = false;
+                bool doorsta=false;
+                unsigned char speedlimte;
+
+        int grade=0;
+        int driverMode=0;
+        int epb=0;
+        int epsMode=1;
+        bool receiveEps=false;
+
+
+              bool speed_control=true;
+              bool group_control =false;
+              int group_server_status=0;
+              int  group_state = 0;
+              float group_x_local = 0.0;
+              float group_y_local = 0.0;
+              float group_velx_local = 0.0;
+              float group_vely_local = 0.0;
+              float group_comm_speed = 0.0;
+              int group_pathpoint=0;
+              float group_offsetx=0.0;
+              float group_frontDistance=0.0;
+
+              float mfttc = 0;
+
+
+        std::vector <iv::platform::station> car_stations;
+
+
+        AWS_display aws_display;
+        lane Lane;
+        obstacle_status obstacleStatus;
+        obstacle_data_a obstacleStatusA[15];
+        obstacle_data_b obstacleStatusB[15];
+        obstacle_data_c obstacleStatusC[15];
+        aftermarket_lane aftermarketLane;
+        lka_left_lane_a LKAleftLaneA;
+        lka_left_lane_b LKAleftLaneB;
+        lka_right_lane_a LKArightLaneA;
+        lka_right_lane_b LKArightLaneB;
+        next_lane_a nextLaneA_left[4], nextLaneA_right[4];
+        next_lane_b nextLaneB_left[4], nextLaneB_right[4];
+        ref_points refPoints;
+        num_of_next_lane_mark_reported numOfNextLaneMarkReported;
+
+
+        double mfAcc = 0;
+        double mfVehAcc=0;
+        double mfWheel = 0;
+        double mfBrake = 0;
+        double steerAngle=0;
+        QTime mRunTime;
+
+        int mRadarS = -1;
+        int mLidarS = -1;
+        int mRTKStatus = 0;
+        int mLidarPer = -1;
+
+        double mLidarObs;
+        double mRadarObs;
+        double mObs;
+
+
+        GPS_INS aim_gps_ins;
+        bool bocheMode=false;
+        int bocheEnable=0;
+
+        double mfParkLat;
+        double mfParkLng;
+        double mfParkHeading;
+        int mnParkType;
+
+
+        double mLidarRotation;
+        double mLidarRangeUnit;
+
+        iv::GPSData location;		//当前车辆位置
+        boost::array<iv::ObstacleBasic, 64> obs_radar;//毫米波雷达的障碍物数据
+         boost::array<iv::ObstacleBasic, 64> obs_rear_radar;//houxiang毫米波雷达的障碍物数据
+        iv::ultrasonic_obs multrasonic_obs;
+        double mfGPSAcc = 0;
+
+//        iv::brain::decitionview mdecitionview;
+        iv::sysparam msysparam;
+        iv::roadmode_vel mroadmode_vel;
+        int vehGroupId;
+        double mfEPSOff = 0.0;
+        float socfDis=15;   //停障触发距离
+        float aocfDis=25;   //避障触发距离
+        float aocfTimes=5; //避障触发次数
+        float aobzDis=5;   //避障保障距离
+        /// traffice light : 0x90
+        int iTrafficeLightID = 0;       //红绿灯ID
+        int iTrafficeLightColor = 2;    //0x0: 红色
+        //0x1: 黄色
+        //0x2: 绿色
+        int iTrafficeLightTime = 0;     //红绿灯剩余时间
+        double iTrafficeLightLat = 0;
+        double iTrafficeLightLon = 0;
+
+        bool daocheMode=false;
+
+        //////////////////////////////////////////////////////
+
+        ///kkcai, 2020-01-03
+        QMutex mMutexMap;
+        /////////////////////////////////////
+
+        //mobileEye chuku
+
+        float vehSpeed_st=0;
+
+        bool useMobileEye = false;
+        bool m_bOutGarage=false;//车道线识别出库标志
+        float outGarageLong=10;
+        float waitGpsTimeWidth=5000;
+
+        int mnBocheComplete = 0;   //When boche comple set a value.
+
+        bool useObsPredict = false;
+
+    };
+    typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
+}
+#define ServiceCarStatus iv::CarStatusSingleton::get_mutable_instance()
+#endif // !_IV_COMMON_CAR_STATUS_

src/decition/common/common/common.pri

@@ -1,43 +1,43 @@
-#pragma once
-#ifndef _IV_COMMON_CONSTANTS_
-#define _IV_COMMON_CONSTANTS_
-
-#include <common/car_status.h>
-namespace iv {
-
-
-    const std::uint8_t SPEED_MAX = 30;						//车速上限
-    const std::uint8_t BRAKING_PRESSURE_PERCENT_MAX = 50;	//刹车压力上限
-    const std::uint8_t ACCELERATE_PERCENT_MAX = 1;			//油门开合度上限
-
-    const std::uint8_t CONTROL_SPEED_TOLERANCE = 5;		//控制车速时容忍误差范围
-    const std::uint8_t CONTROL_WHEEL_ANGLE_TOLERANCE = 5;	//控制方向盘时容忍误差范围
-
-    const std::uint16_t WHEEL_ZERO = 0x1F4A;
-    //static iv::CarStatus CarStatus_(new iv::CarStatusBasic);
-
-    const float SPEED_RATIO = 0.08;		//实际车速和can中读出的车速比率
-
-    const float Veh_Width = 2.1;		//车宽
-    const float Veh_Lenth = 4.6;
-    const float Esr_Offset = 0;		//ESR偏移量
-    const float Esr_Y_Offset = 2.5;  //ESR纵向偏移量
-
-    const std::uint8_t THREAD_EXECUTECONTROL_MAXNUM = 1;	//decition_executer 执行线程最大数量
-
-
-    const double MaxValue = 1.7976931348623157E+308;  //     Represents the largest possible value of a System.Double. This field is constant.
-
-
-    const double MinValue = -1.7976931348623157E+308; //     Represents the smallest possible value of a System.Double. This field is constant.
-
-
-
-
-
-
-}
-
-
-
-#endif // !_IV_COMMON_CONSTANTS_
+#pragma once
+#ifndef _IV_COMMON_CONSTANTS_
+#define _IV_COMMON_CONSTANTS_
+
+#include <common/car_status.h>
+namespace iv {
+
+
+    const std::uint8_t SPEED_MAX = 30;						//车速上限
+    const std::uint8_t BRAKING_PRESSURE_PERCENT_MAX = 50;	//刹车压力上限
+    const std::uint8_t ACCELERATE_PERCENT_MAX = 1;			//油门开合度上限
+
+    const std::uint8_t CONTROL_SPEED_TOLERANCE = 5;		//控制车速时容忍误差范围
+    const std::uint8_t CONTROL_WHEEL_ANGLE_TOLERANCE = 5;	//控制方向盘时容忍误差范围
+
+    const std::uint16_t WHEEL_ZERO = 0x1F4A;
+    //static iv::CarStatus CarStatus_(new iv::CarStatusBasic);
+
+    const float SPEED_RATIO = 0.08;		//实际车速和can中读出的车速比率
+
+    const float Veh_Width = 2.1;		//车宽
+    const float Veh_Lenth = 4.6;
+    const float Esr_Offset = 0;		//ESR偏移量
+    const float Esr_Y_Offset = 2.5;  //ESR纵向偏移量
+
+    const std::uint8_t THREAD_EXECUTECONTROL_MAXNUM = 1;	//decition_executer 执行线程最大数量
+
+
+    const double MaxValue = 1.7976931348623157E+308;  //     Represents the largest possible value of a System.Double. This field is constant.
+
+
+    const double MinValue = -1.7976931348623157E+308; //     Represents the smallest possible value of a System.Double. This field is constant.
+
+
+
+
+
+
+}
+
+
+
+#endif // !_IV_COMMON_CONSTANTS_

src/decition/common/common/gps_type.h

@@ -1,99 +1,99 @@
-#pragma once
-/*
-*GPS 惯导数据
-*/
-#ifndef _IV_COMMON_GPS_TYPE_
-#define _IV_COMMON_GPS_TYPE_
-
-#include <common/boost.h>
-namespace iv {
-    struct GPS_INS
-    {
-        int valid = 0xff;
-        int index = 0;	//gps点序号
-
-        double gps_lat = 0;//纬度
-        double gps_lng = 0;//经度
-
-        double gps_x = 0;
-        double gps_y = 0;
-        double gps_z = 0;
-
-        double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
-        double ins_pitch_angle = 0;	//俯仰角
-        double ins_heading_angle = 0;	//航向角
-
-        int ins_status = 0;	//惯导状态 4
-        int rtk_status = 0;	//rtk状态 6 -5 -3
-        int gps_satelites_num = 0;
-
-        //-----加速度--------------
-        double accel_x = 0;
-        double accel_y = 0;
-        double accel_z = 0;
-
-        //-------角速度------------
-        double ang_rate_x = 0;
-        double ang_rate_y = 0;
-        double ang_rate_z = 0;
-
-        //-----------方向速度--------------
-        double vel_N = 0;
-        double vel_E = 0;
-        double vel_D = 0;
-
-        int speed_mode = 0;
-        int mode2 = 0;
-        double speed = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
-
-        int roadMode;
-        int runMode;
-        int roadSum;
-        int roadOri;
-
-
-
-    };
-
-//    typedef std::shared_ptr<iv::GPS_INS> GPSData;
-    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
-         int roadMode = 0;
-
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
-
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
-
-
-     };
-
-
-     class TrafficLight
-    {
-      public:
-         int leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
-         int leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
-
-         TrafficLight()
-        {
-            leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
-            leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
-        }
-
-     };
-
-
-
-
-}
-#endif // !_IV_COMMON_GPS_TYPE_
+#pragma once
+/*
+*GPS 惯导数据
+*/
+#ifndef _IV_COMMON_GPS_TYPE_
+#define _IV_COMMON_GPS_TYPE_
+
+#include <common/boost.h>
+namespace iv {
+    struct GPS_INS
+    {
+        int valid = 0xff;
+        int index = 0;	//gps点序号
+
+        double gps_lat = 0;//纬度
+        double gps_lng = 0;//经度
+
+        double gps_x = 0;
+        double gps_y = 0;
+        double gps_z = 0;
+
+        double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
+        double ins_pitch_angle = 0;	//俯仰角
+        double ins_heading_angle = 0;	//航向角
+
+        int ins_status = 0;	//惯导状态 4
+        int rtk_status = 0;	//rtk状态 6 -5 -3
+        int gps_satelites_num = 0;
+
+        //-----加速度--------------
+        double accel_x = 0;
+        double accel_y = 0;
+        double accel_z = 0;
+
+        //-------角速度------------
+        double ang_rate_x = 0;
+        double ang_rate_y = 0;
+        double ang_rate_z = 0;
+
+        //-----------方向速度--------------
+        double vel_N = 0;
+        double vel_E = 0;
+        double vel_D = 0;
+
+        int speed_mode = 0;
+        int mode2 = 0;
+        double speed = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        int roadMode;
+        int runMode;
+        int roadSum;
+        int roadOri;
+
+
+
+    };
+
+//    typedef std::shared_ptr<iv::GPS_INS> GPSData;
+    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
+     class Point2D
+    {
+      public:
+          double x = 0, y = 0, speed=0;
+         int v1 = 0, v2 = 0;
+         int roadMode = 0;
+
+         Point2D()
+        {
+            x = y = v1 = 0;
+        }
+
+         Point2D(double _x, double _y)
+        {
+            x = _x; y = _y;
+        }
+
+
+     };
+
+
+     class TrafficLight
+    {
+      public:
+         int leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+         int leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+
+         TrafficLight()
+        {
+            leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+            leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+        }
+
+     };
+
+
+
+
+}
+#endif // !_IV_COMMON_GPS_TYPE_

src/decition/common/common/group_type.h

@@ -1,84 +1,84 @@
-#include "lidar.h"
-
-iv::Lidar::Lidar()
-{
-	Lidar_Grid = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-    Lidar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-
-    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> pertem(new std::vector<iv::Perception::PerceptionOutput>);
-    mper = pertem;
-}
-
-iv::Lidar::~Lidar()
-{
-}
-
-void iv::Lidar::copyfromlidar(iv::ObsLiDAR& obs)
-{
-    is_run = true;
-    if(mtx1.try_lock() == true)
-    {
-        Lidar_->clear();
-        Lidar_.swap(obs);
-        genggai1 = true;
-        mtx1.unlock();
-    }
-}
-void iv::Lidar::copylidarto(iv::ObsLiDAR& obs)
-{
-	if (genggai1 == true)
-	{
-        mtx1.lock();
-        obs->clear();
-        Lidar_.swap(obs);
-		genggai1 = false;
-		mtx1.unlock();
-	}
-}
-
-void iv::Lidar::copyfromlidarobs(iv::ObsLiDAR& obs)
-{
-    if(mtx2.try_lock() == true)
-    {
-        Lidar_Grid->clear();
-        Lidar_Grid.swap(obs);
-        genggai2 = true;
-        mtx2.unlock();
-    }
-}
-void iv::Lidar::copylidarobsto(iv::ObsLiDAR& obs)
-{
-	if (genggai2 == true)
-	{
-		mtx2.lock();
-		obs->clear();
-		Lidar_Grid.swap(obs);
-		genggai2 = false;
-		mtx2.unlock();
-	}
-}
-
-void iv::Lidar::copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > & per)
-{
-    mMutexper.lock();
-    mper.swap(per);
-    mMutexper.unlock();
-}
-
-void iv::Lidar::copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > & per)
-{
- //   int nSize = mper->size();
-    mMutexper.lock();
-//    per.swap(mper);
-    mper.swap(per);
-    mMutexper.unlock();
-//    if(nSize > 0)qDebug("have per");
-}
-
-
-bool iv::Lidar::did_lidar_ok()
-{
-    bool temp = is_run;
-    is_run = false;
-    return temp;
-}
+#include "lidar.h"
+
+iv::Lidar::Lidar()
+{
+	Lidar_Grid = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+    Lidar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+
+    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> pertem(new std::vector<iv::Perception::PerceptionOutput>);
+    mper = pertem;
+}
+
+iv::Lidar::~Lidar()
+{
+}
+
+void iv::Lidar::copyfromlidar(iv::ObsLiDAR& obs)
+{
+    is_run = true;
+    if(mtx1.try_lock() == true)
+    {
+        Lidar_->clear();
+        Lidar_.swap(obs);
+        genggai1 = true;
+        mtx1.unlock();
+    }
+}
+void iv::Lidar::copylidarto(iv::ObsLiDAR& obs)
+{
+	if (genggai1 == true)
+	{
+        mtx1.lock();
+        obs->clear();
+        Lidar_.swap(obs);
+		genggai1 = false;
+		mtx1.unlock();
+	}
+}
+
+void iv::Lidar::copyfromlidarobs(iv::ObsLiDAR& obs)
+{
+    if(mtx2.try_lock() == true)
+    {
+        Lidar_Grid->clear();
+        Lidar_Grid.swap(obs);
+        genggai2 = true;
+        mtx2.unlock();
+    }
+}
+void iv::Lidar::copylidarobsto(iv::ObsLiDAR& obs)
+{
+	if (genggai2 == true)
+	{
+		mtx2.lock();
+		obs->clear();
+		Lidar_Grid.swap(obs);
+		genggai2 = false;
+		mtx2.unlock();
+	}
+}
+
+void iv::Lidar::copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > & per)
+{
+    mMutexper.lock();
+    mper.swap(per);
+    mMutexper.unlock();
+}
+
+void iv::Lidar::copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > & per)
+{
+ //   int nSize = mper->size();
+    mMutexper.lock();
+//    per.swap(mper);
+    mper.swap(per);
+    mMutexper.unlock();
+//    if(nSize > 0)qDebug("have per");
+}
+
+
+bool iv::Lidar::did_lidar_ok()
+{
+    bool temp = is_run;
+    is_run = false;
+    return temp;
+}

src/decition/common/common/lidar.h

@@ -1,39 +1,39 @@
-#pragma once
-#ifndef _IV_COMMON_LIDAR_
-#define _IV_COMMON_LIDAR_
-
-#include <common/boost.h>
-#include <boost/serialization/singleton.hpp>
-#include <common/obstacle_type.h>
-#include <common/car_status.h>
-
-#include <QMutex>
-
-#include "perception/perceptionoutput.h"
-
-namespace iv {
-	class Lidar : public boost::noncopyable {
-	public:
-		Lidar();
-		~Lidar();
-		bool genggai1 = false, genggai2 = false;
-        bool is_run = false;
-		boost::mutex mtx1, mtx2;
-		iv::ObsLiDAR Lidar_Grid;
-        iv::ObsLiDAR Lidar_;
-        QMutex mMutexper;
-        std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> mper;
-
-        void copyfromlidar(iv::ObsLiDAR& obs);
-        void copylidarto(iv::ObsLiDAR& obs);
-		void copyfromlidarobs(iv::ObsLiDAR& obs);
-        void copylidarobsto(iv::ObsLiDAR& obs);
-
-        void copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
-        void copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
-        bool did_lidar_ok();
-	};
-	typedef boost::serialization::singleton<iv::Lidar> LidarSingleton;
-}
-#define ServiceLidar iv::LidarSingleton::get_mutable_instance()
-#endif // !_IV_COMMON_LIDAR_
+#pragma once
+#ifndef _IV_COMMON_LIDAR_
+#define _IV_COMMON_LIDAR_
+
+#include <common/boost.h>
+#include <boost/serialization/singleton.hpp>
+#include <common/obstacle_type.h>
+#include <common/car_status.h>
+
+#include <QMutex>
+
+#include "perception/perceptionoutput.h"
+
+namespace iv {
+	class Lidar : public boost::noncopyable {
+	public:
+		Lidar();
+		~Lidar();
+		bool genggai1 = false, genggai2 = false;
+        bool is_run = false;
+		boost::mutex mtx1, mtx2;
+		iv::ObsLiDAR Lidar_Grid;
+        iv::ObsLiDAR Lidar_;
+        QMutex mMutexper;
+        std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> mper;
+
+        void copyfromlidar(iv::ObsLiDAR& obs);
+        void copylidarto(iv::ObsLiDAR& obs);
+		void copyfromlidarobs(iv::ObsLiDAR& obs);
+        void copylidarobsto(iv::ObsLiDAR& obs);
+
+        void copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
+        void copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
+        bool did_lidar_ok();
+	};
+	typedef boost::serialization::singleton<iv::Lidar> LidarSingleton;
+}
+#define ServiceLidar iv::LidarSingleton::get_mutable_instance()
+#endif // !_IV_COMMON_LIDAR_

src/decition/common/common/logout.h

@@ -1,136 +1,136 @@
-/**
-* 用于输出log文件的类.
-*/
-
-
-#ifndef LOG_H      
-#define LOG_H      
-
-
-//log文件路径    
-#define LOG_FILE_NAME "log_319.txt"    
-
-//启用开关    
-#define LOG_ENABLE    
-
-#include <fstream>      
-#include <string>      
-#include <ctime>
-//#include <Windows.h>
-//#include <tchar.h>
-/*
-using namespace std;
-
-class CLog
-{
-public:
-	static void GetLogFilePath(CHAR* szPath)
-	{
-		GetModuleFileNameA(NULL, szPath, MAX_PATH);
-		memset(strrchr(szPath, _T('\\')), NULL, strlen(strrchr(szPath, _T('\\'))) * sizeof(CHAR));
-		strcat(szPath, "\\");
-		strcat(szPath, LOG_FILE_NAME);
-	}
-	//输出一个内容，可以是字符串(ascii)、整数、浮点数、布尔、枚举    
-	//格式为：[2011-11-11 11:11:11] aaaaaaa并换行    
-	template <class T>
-	static void WriteLog(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x << endl;
-		fout.close();
-	}
-
-	//输出2个内容，以等号连接。一般用于前面是一个变量的描述字符串，后面接这个变量的值    
-	template<class T1, class T2>
-	static void WriteLog2(T1 x1, T2 x2)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x1 << " = " << x2 << endl;
-		fout.close();
-	}
-
-	template<class T1, class T2, class T3, class T4, class T5, class T6 >
-	static void WriteLog6(T1 x1, T2 x2, T3 x3, T4 x4, T5 x5, T6 x6)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x1 << "  " << x2 << "  "  
-								<< x3 << "  " << x4 << "  " 
-								<< x5 << "  " << x6 << endl;
-		fout.close();
-	}
-
-	//输出一行当前函数开始的标志,宏传入__FUNCTION__    
-	template <class T>
-	static void WriteFuncBegin(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << "    --------------------" << x << "  Begin--------------------" << endl;
-		fout.close();
-	}
-
-	//输出一行当前函数结束的标志，宏传入__FUNCTION__    
-	template <class T>
-	static void WriteFuncEnd(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << "--------------------" << x << "  End  --------------------" << endl;
-		fout.close();
-	}
-
-
-private:
-	//获取本地时间，格式如"[2011-11-11 11:11:11] ";     
-	static string GetSystemTime()
-	{
-		time_t tNowTime;
-		time(&tNowTime);
-		tm* tLocalTime = localtime(&tNowTime);
-		char szTime[30] = { '\0' };
-		strftime(szTime, 30, "[%Y-%m-%d %H:%M:%S] ", tLocalTime);
-		string strTime = szTime;
-		return strTime;
-	}
-
-};
-
-#ifdef LOG_ENABLE    
-
-//用下面这些宏来使用本文件    
-#define LOG(x)              CLog::WriteLog(x);          //括号内可以是字符串(ascii)、整数、浮点数、bool等    
-#define LOG2(x1,x2)     CLog::WriteLog2(x1,x2);    
-#define LOG6(x1,x2,x3,x4,x5,x6)		CLog::WriteLog6(x1,x2,x3,x4,x5,x6);
-#define LOG_FUNC        LOG(__FUNCTION__)               //输出当前所在函数名    
-#define LOG_LINE        LOG(__LINE__)                       //输出当前行号    
-#define LOG_FUNC_BEGIN  CLog::WriteFuncBegin(__FUNCTION__);     //形式如：[时间]"------------FuncName  Begin------------"    
-#define LOG_FUNC_END     CLog::WriteFuncEnd(__FUNCTION__);      //形式如：[时间]"------------FuncName  End------------"    
-
-#else    
-
-#define LOG(x)                  
-#define LOG2(x1,x2)         
-#define LOG6(x1,x2,x3,x4,x5,x6)
-#define LOG_FUNC            
-#define LOG_LINE            
-#define LOG_FUNC_BEGIN      
-#define LOG_FUNC_END        
-
-#endif    
-*/
-#endif      
+/**
+* 用于输出log文件的类.
+*/
+
+
+#ifndef LOG_H      
+#define LOG_H      
+
+
+//log文件路径    
+#define LOG_FILE_NAME "log_319.txt"    
+
+//启用开关    
+#define LOG_ENABLE    
+
+#include <fstream>      
+#include <string>      
+#include <ctime>
+//#include <Windows.h>
+//#include <tchar.h>
+/*
+using namespace std;
+
+class CLog
+{
+public:
+	static void GetLogFilePath(CHAR* szPath)
+	{
+		GetModuleFileNameA(NULL, szPath, MAX_PATH);
+		memset(strrchr(szPath, _T('\\')), NULL, strlen(strrchr(szPath, _T('\\'))) * sizeof(CHAR));
+		strcat(szPath, "\\");
+		strcat(szPath, LOG_FILE_NAME);
+	}
+	//输出一个内容，可以是字符串(ascii)、整数、浮点数、布尔、枚举    
+	//格式为：[2011-11-11 11:11:11] aaaaaaa并换行    
+	template <class T>
+	static void WriteLog(T x)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << x << endl;
+		fout.close();
+	}
+
+	//输出2个内容，以等号连接。一般用于前面是一个变量的描述字符串，后面接这个变量的值    
+	template<class T1, class T2>
+	static void WriteLog2(T1 x1, T2 x2)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << x1 << " = " << x2 << endl;
+		fout.close();
+	}
+
+	template<class T1, class T2, class T3, class T4, class T5, class T6 >
+	static void WriteLog6(T1 x1, T2 x2, T3 x3, T4 x4, T5 x5, T6 x6)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << x1 << "  " << x2 << "  "  
+								<< x3 << "  " << x4 << "  " 
+								<< x5 << "  " << x6 << endl;
+		fout.close();
+	}
+
+	//输出一行当前函数开始的标志,宏传入__FUNCTION__    
+	template <class T>
+	static void WriteFuncBegin(T x)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << "    --------------------" << x << "  Begin--------------------" << endl;
+		fout.close();
+	}
+
+	//输出一行当前函数结束的标志，宏传入__FUNCTION__    
+	template <class T>
+	static void WriteFuncEnd(T x)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << "--------------------" << x << "  End  --------------------" << endl;
+		fout.close();
+	}
+
+
+private:
+	//获取本地时间，格式如"[2011-11-11 11:11:11] ";     
+	static string GetSystemTime()
+	{
+		time_t tNowTime;
+		time(&tNowTime);
+		tm* tLocalTime = localtime(&tNowTime);
+		char szTime[30] = { '\0' };
+		strftime(szTime, 30, "[%Y-%m-%d %H:%M:%S] ", tLocalTime);
+		string strTime = szTime;
+		return strTime;
+	}
+
+};
+
+#ifdef LOG_ENABLE    
+
+//用下面这些宏来使用本文件    
+#define LOG(x)              CLog::WriteLog(x);          //括号内可以是字符串(ascii)、整数、浮点数、bool等    
+#define LOG2(x1,x2)     CLog::WriteLog2(x1,x2);    
+#define LOG6(x1,x2,x3,x4,x5,x6)		CLog::WriteLog6(x1,x2,x3,x4,x5,x6);
+#define LOG_FUNC        LOG(__FUNCTION__)               //输出当前所在函数名    
+#define LOG_LINE        LOG(__LINE__)                       //输出当前行号    
+#define LOG_FUNC_BEGIN  CLog::WriteFuncBegin(__FUNCTION__);     //形式如：[时间]"------------FuncName  Begin------------"    
+#define LOG_FUNC_END     CLog::WriteFuncEnd(__FUNCTION__);      //形式如：[时间]"------------FuncName  End------------"    
+
+#else    
+
+#define LOG(x)                  
+#define LOG2(x1,x2)         
+#define LOG6(x1,x2,x3,x4,x5,x6)
+#define LOG_FUNC            
+#define LOG_LINE            
+#define LOG_FUNC_BEGIN      
+#define LOG_FUNC_END        
+
+#endif    
+*/
+#endif      

src/decition/common/common/obstacle_type.h

@@ -1,67 +1,67 @@
-#pragma once
-#ifndef _IV_COMMON_OBSTACLE_TYPE_
-#define _IV_COMMON_OBSTACLE_TYPE_
-#include <vector>
-#include <common/boost.h>
-/**
-*障碍物类型
-*/
-namespace iv {
-    const int grx = 250, gry = 500, centerx = 125, centery = 250;
-    const double gridwide = 0.2;
-    struct ObstacleBasic
-    {
-        bool valid;
-        float nomal_x;
-        float nomal_y;
-        float nomal_z;
-
-        float speed_relative;
-        float speed_x;
-        float speed_y;
-        float speed_z;
-
-        float high;
-        float low;
-
-        int esr_ID;
-    };
-
-    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsLiDAR;
-    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsRadar;
-    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsCamera;
-    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>>ObsRadarPointer;
-    struct Obs_grid
-    {
-        int  id;
-        double speed_x;
-        double speed_y;
-        double yaw;
-        int type;
-        double high;
-        double low;
-        double obshight;
-        int pointcount;
-        int ob;//障碍物属性，0地面 ，1路沿，2障碍物
-    };
-
-    typedef Obs_grid LidarGrid;
-    typedef Obs_grid* LidarGridPtr;
-
-    struct array_360
-    {
-        float x;
-        float y;
-        float z;
-    };
-
-    struct detect_info
-    {
-        int light;
-    };
-
-    typedef detect_info CameraInfo;
-    typedef detect_info* CameraInfoPtr;
-}
-
-#endif // !_IV_COMMON_OBSTACLE_TYPE_
+#pragma once
+#ifndef _IV_COMMON_OBSTACLE_TYPE_
+#define _IV_COMMON_OBSTACLE_TYPE_
+#include <vector>
+#include <common/boost.h>
+/**
+*障碍物类型
+*/
+namespace iv {
+    const int grx = 250, gry = 500, centerx = 125, centery = 250;
+    const double gridwide = 0.2;
+    struct ObstacleBasic
+    {
+        bool valid;
+        float nomal_x;
+        float nomal_y;
+        float nomal_z;
+
+        float speed_relative;
+        float speed_x;
+        float speed_y;
+        float speed_z;
+
+        float high;
+        float low;
+
+        int esr_ID;
+    };
+
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsLiDAR;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsRadar;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsCamera;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>>ObsRadarPointer;
+    struct Obs_grid
+    {
+        int  id;
+        double speed_x;
+        double speed_y;
+        double yaw;
+        int type;
+        double high;
+        double low;
+        double obshight;
+        int pointcount;
+        int ob;//障碍物属性，0地面 ，1路沿，2障碍物
+    };
+
+    typedef Obs_grid LidarGrid;
+    typedef Obs_grid* LidarGridPtr;
+
+    struct array_360
+    {
+        float x;
+        float y;
+        float z;
+    };
+
+    struct detect_info
+    {
+        int light;
+    };
+
+    typedef detect_info CameraInfo;
+    typedef detect_info* CameraInfoPtr;
+}
+
+#endif // !_IV_COMMON_OBSTACLE_TYPE_

src/decition/common/common/platform_type.h

@@ -1,51 +1,51 @@
-#pragma once
-
-#ifndef _IV_BOOST_H_
-#define _IV_BOOST_H_
-
-#if defined __GNUC__
-#  pragma GCC system_header 
-#endif
-#ifndef __CUDACC__
-//https://bugreports.qt-project.org/browse/QTBUG-22829
-#ifndef Q_MOC_RUN
-#include <boost/version.hpp>
-#include <boost/numeric/conversion/cast.hpp>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/condition.hpp>
-#include <boost/thread.hpp>
-#include <boost/thread/thread.hpp>
-#include <boost/filesystem.hpp>
-#include <boost/bind.hpp>
-#include <boost/cstdint.hpp>
-#include <boost/function.hpp>
-#include <boost/tuple/tuple.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include <boost/mpl/fold.hpp>
-#include <boost/mpl/inherit.hpp>
-#include <boost/mpl/inherit_linearly.hpp>
-#include <boost/mpl/joint_view.hpp>
-#include <boost/mpl/transform.hpp>
-#include <boost/mpl/vector.hpp>
-#include <boost/algorithm/string.hpp>
-#ifndef Q_MOC_RUN
-#include <boost/date_time/posix_time/posix_time.hpp>
-#endif
-#if BOOST_VERSION >= 104700
-#include <boost/chrono.hpp>
-#endif
-#include <boost/tokenizer.hpp>
-#include <boost/foreach.hpp>
-#include <boost/shared_array.hpp>
-#include <boost/interprocess/sync/file_lock.hpp>
-#if BOOST_VERSION >= 104900
-#include <boost/interprocess/permissions.hpp>
-#endif
-#include <boost/iostreams/device/mapped_file.hpp>
-#define BOOST_PARAMETER_MAX_ARITY 7
-#include <boost/signals2.hpp>
-#include <boost/signals2/slot.hpp>
-#endif
-#endif
-#endif    // _IV_BOOST_H_
+#pragma once
+
+#ifndef _IV_BOOST_H_
+#define _IV_BOOST_H_
+
+#if defined __GNUC__
+#  pragma GCC system_header 
+#endif
+#ifndef __CUDACC__
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/bind.hpp>
+#include <boost/cstdint.hpp>
+#include <boost/function.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/mpl/fold.hpp>
+#include <boost/mpl/inherit.hpp>
+#include <boost/mpl/inherit_linearly.hpp>
+#include <boost/mpl/joint_view.hpp>
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/vector.hpp>
+#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
+#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
+#include <boost/chrono.hpp>
+#endif
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/shared_array.hpp>
+#include <boost/interprocess/sync/file_lock.hpp>
+#if BOOST_VERSION >= 104900
+#include <boost/interprocess/permissions.hpp>
+#endif
+#include <boost/iostreams/device/mapped_file.hpp>
+#define BOOST_PARAMETER_MAX_ARITY 7
+#include <boost/signals2.hpp>
+#include <boost/signals2/slot.hpp>
+#endif
+#endif
+#endif    // _IV_BOOST_H_

src/decition/common/common_sf/car_status.cpp

@@ -1,24 +1,24 @@
-#include "car_status.h"
-
-#ifdef linux
-
-unsigned long GetTickCount()
-{
- struct timespec ts;
- clock_gettime(CLOCK_MONOTONIC,&ts);
- return (ts.tv_sec * 1000 + ts.tv_nsec/(1000*1000) );
-}
-#endif
-
-iv::CarStatus::CarStatus()
-{
-	speed = 0;
-	braking_pressure = 0;
-	wheel_angle = 0;
-	location = boost::shared_ptr<iv::GPS_INS>(new iv::GPS_INS);
-    mRunTime.start();
-}
-
-iv::CarStatus::~CarStatus()
-{
-}
+#include "car_status.h"
+
+#ifdef linux
+
+unsigned long GetTickCount()
+{
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC,&ts);
+ return (ts.tv_sec * 1000 + ts.tv_nsec/(1000*1000) );
+}
+#endif
+
+iv::CarStatus::CarStatus()
+{
+	speed = 0;
+	braking_pressure = 0;
+	wheel_angle = 0;
+	location = boost::shared_ptr<iv::GPS_INS>(new iv::GPS_INS);
+    mRunTime.start();
+}
+
+iv::CarStatus::~CarStatus()
+{
+}

src/decition/common/common_sf/car_status.h

@@ -1,188 +1,188 @@
-#pragma once
-#ifndef _IV_COMMON_CAR_STATUS_
-#define _IV_COMMON_CAR_STATUS_
-
-#include <common/boost.h>
-#include <boost/serialization/singleton.hpp>
-#include <common/gps_type.h>
-#include <common/obstacle_type.h>
-#include <time.h>
-#include <control/mobileye.h>
-#include <QTime>
-
-///kkcai, 2020-01-03
-#include <QMutex>
-///////////////////////////////////////
-
-#include "platform/platform.h"
-
-#include "ultrasonic_type.h"
-
-//#include "common/decitionview.pb.h"
-
-#include "common/sysparam_type.h"
-
-#include "common/roadmode_type.h"
-
-#define RADAR_OBJ_NUM 64
-
-#ifdef linux
-unsigned long GetTickCount();
-#endif
-namespace iv {
-    class CarStatus : public boost::noncopyable {
-    public:
-        CarStatus();
-        ~CarStatus();
-
-        float speed;			//车速
-        double mfCalAcc = 0;
-        std::int16_t wheel_angle;		//方向盘转角
-        std::uint8_t braking_pressure;	//刹车压力
-         float torque_st=0;
-        bool mbRunPause = true;
-        bool mbBrainCtring = false;
-        bool status[6] = { true, false, false, false, true, false };	//bool arrive = false;	//  x4:是否到达站点（0:未到 1:到达）
-                                                                //bool people = false;	//	x3:车上是否有人（0:无人 1:有人）
-                                                                //bool stop = false;	    //	x2:是否停车（0:否   1:是）
-                                                                //bool call = false;		//	x1:是否叫车（0:否	1:是）
-                                                                //bool available = true;	// 是否可被叫车
-                                                                //bool fire = false;
- //       int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
-
-        int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
-                int istostation = 0;
-                //int ctostation = 0;
-                int currentstation = 0;
-                //int nextstation = 0;
-                int clientto = 0;
-                double amilng = 0.0;
-                double amilat = 0.0;
-                bool busmode = false;
-                bool netconnect = false;
-                bool doorsta=false;
-                unsigned char speedlimte;
-
-        int grade=0;
-        int driverMode=0;
-        int epb=0;
-        int epsMode=1;
-        bool receiveEps=false;
-
-
-              bool speed_control=true;
-              bool group_control =false;
-              int group_server_status=0;
-              int  group_state = 0;
-              float group_x_local = 0.0;
-              float group_y_local = 0.0;
-              float group_velx_local = 0.0;
-              float group_vely_local = 0.0;
-              float group_comm_speed = 0.0;
-              int group_pathpoint=0;
-              float group_offsetx=0.0;
-              float group_frontDistance=0.0;
-
-              float mfttc = 0;
-
-
-        std::vector <iv::platform::station> car_stations;
-
-
-        AWS_display aws_display;
-        lane Lane;
-        obstacle_status obstacleStatus;
-        obstacle_data_a obstacleStatusA[15];
-        obstacle_data_b obstacleStatusB[15];
-        obstacle_data_c obstacleStatusC[15];
-        aftermarket_lane aftermarketLane;
-        lka_left_lane_a LKAleftLaneA;
-        lka_left_lane_b LKAleftLaneB;
-        lka_right_lane_a LKArightLaneA;
-        lka_right_lane_b LKArightLaneB;
-        next_lane_a nextLaneA_left[4], nextLaneA_right[4];
-        next_lane_b nextLaneB_left[4], nextLaneB_right[4];
-        ref_points refPoints;
-        num_of_next_lane_mark_reported numOfNextLaneMarkReported;
-
-
-        double mfAcc = 0;
-        double mfVehAcc=0;
-        double mfWheel = 0;
-        double mfBrake = 0;
-        double steerAngle=0;
-        QTime mRunTime;
-
-        int mRadarS = -1;
-        int mLidarS = -1;
-        int mRTKStatus = 0;
-        int mLidarPer = -1;
-
-        double mLidarObs;
-        double mRadarObs;
-        double mObs;
-
-
-        GPS_INS aim_gps_ins;
-        bool bocheMode=false;
-        int bocheEnable=0;
-
-        double mfParkLat;
-        double mfParkLng;
-        double mfParkHeading;
-        int mnParkType;
-
-
-        double mLidarRotation;
-        double mLidarRangeUnit;
-
-        iv::GPSData location;		//当前车辆位置
-        boost::array<iv::ObstacleBasic, 64> obs_radar;//毫米波雷达的障碍物数据
-         boost::array<iv::ObstacleBasic, 64> obs_rear_radar;//houxiang毫米波雷达的障碍物数据
-        iv::ultrasonic_obs multrasonic_obs;
-        double mfGPSAcc = 0;
-
-//        iv::brain::decitionview mdecitionview;
-        iv::sysparam msysparam;
-        iv::roadmode_vel mroadmode_vel;
-        int vehGroupId;
-        double mfEPSOff = 0.0;
-        float socfDis=15;   //停障触发距离
-        float aocfDis=25;   //避障触发距离
-        float aocfTimes=5; //避障触发次数
-        float aobzDis=5;   //避障保障距离
-        /// traffice light : 0x90
-        int iTrafficeLightID = 0;       //红绿灯ID
-        int iTrafficeLightColor = 2;    //0x0: 红色
-        //0x1: 黄色
-        //0x2: 绿色
-        int iTrafficeLightTime = 0;     //红绿灯剩余时间
-        double iTrafficeLightLat = 0;
-        double iTrafficeLightLon = 0;
-
-        bool daocheMode=false;
-
-        //////////////////////////////////////////////////////
-
-        ///kkcai, 2020-01-03
-        QMutex mMutexMap;
-        /////////////////////////////////////
-
-        //mobileEye chuku
-
-        float vehSpeed_st=0;
-
-        bool useMobileEye = false;
-        bool m_bOutGarage=false;//车道线识别出库标志
-        float outGarageLong=10;
-        float waitGpsTimeWidth=5000;
-
-        int mnBocheComplete = 0;   //When boche comple set a value.
-
-        bool useObsPredict = false;
-
-    };
-    typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
-}
-#define ServiceCarStatus iv::CarStatusSingleton::get_mutable_instance()
-#endif // !_IV_COMMON_CAR_STATUS_
+#pragma once
+#ifndef _IV_COMMON_CAR_STATUS_
+#define _IV_COMMON_CAR_STATUS_
+
+#include <common/boost.h>
+#include <boost/serialization/singleton.hpp>
+#include <common/gps_type.h>
+#include <common/obstacle_type.h>
+#include <time.h>
+#include <control/mobileye.h>
+#include <QTime>
+
+///kkcai, 2020-01-03
+#include <QMutex>
+///////////////////////////////////////
+
+#include "platform/platform.h"
+
+#include "ultrasonic_type.h"
+
+//#include "common/decitionview.pb.h"
+
+#include "common/sysparam_type.h"
+
+#include "common/roadmode_type.h"
+
+#define RADAR_OBJ_NUM 64
+
+#ifdef linux
+unsigned long GetTickCount();
+#endif
+namespace iv {
+    class CarStatus : public boost::noncopyable {
+    public:
+        CarStatus();
+        ~CarStatus();
+
+        float speed;			//车速
+        double mfCalAcc = 0;
+        std::int16_t wheel_angle;		//方向盘转角
+        std::uint8_t braking_pressure;	//刹车压力
+         float torque_st=0;
+        bool mbRunPause = true;
+        bool mbBrainCtring = false;
+        bool status[6] = { true, false, false, false, true, false };	//bool arrive = false;	//  x4:是否到达站点（0:未到 1:到达）
+                                                                //bool people = false;	//	x3:车上是否有人（0:无人 1:有人）
+                                                                //bool stop = false;	    //	x2:是否停车（0:否   1:是）
+                                                                //bool call = false;		//	x1:是否叫车（0:否	1:是）
+                                                                //bool available = true;	// 是否可被叫车
+                                                                //bool fire = false;
+ //       int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
+
+        int carState = 0;	// 0:停车	1:正常循迹	2:前往站点
+                int istostation = 0;
+                //int ctostation = 0;
+                int currentstation = 0;
+                //int nextstation = 0;
+                int clientto = 0;
+                double amilng = 0.0;
+                double amilat = 0.0;
+                bool busmode = false;
+                bool netconnect = false;
+                bool doorsta=false;
+                unsigned char speedlimte;
+
+        int grade=0;
+        int driverMode=0;
+        int epb=0;
+        int epsMode=1;
+        bool receiveEps=false;
+
+
+              bool speed_control=true;
+              bool group_control =false;
+              int group_server_status=0;
+              int  group_state = 0;
+              float group_x_local = 0.0;
+              float group_y_local = 0.0;
+              float group_velx_local = 0.0;
+              float group_vely_local = 0.0;
+              float group_comm_speed = 0.0;
+              int group_pathpoint=0;
+              float group_offsetx=0.0;
+              float group_frontDistance=0.0;
+
+              float mfttc = 0;
+
+
+        std::vector <iv::platform::station> car_stations;
+
+
+        AWS_display aws_display;
+        lane Lane;
+        obstacle_status obstacleStatus;
+        obstacle_data_a obstacleStatusA[15];
+        obstacle_data_b obstacleStatusB[15];
+        obstacle_data_c obstacleStatusC[15];
+        aftermarket_lane aftermarketLane;
+        lka_left_lane_a LKAleftLaneA;
+        lka_left_lane_b LKAleftLaneB;
+        lka_right_lane_a LKArightLaneA;
+        lka_right_lane_b LKArightLaneB;
+        next_lane_a nextLaneA_left[4], nextLaneA_right[4];
+        next_lane_b nextLaneB_left[4], nextLaneB_right[4];
+        ref_points refPoints;
+        num_of_next_lane_mark_reported numOfNextLaneMarkReported;
+
+
+        double mfAcc = 0;
+        double mfVehAcc=0;
+        double mfWheel = 0;
+        double mfBrake = 0;
+        double steerAngle=0;
+        QTime mRunTime;
+
+        int mRadarS = -1;
+        int mLidarS = -1;
+        int mRTKStatus = 0;
+        int mLidarPer = -1;
+
+        double mLidarObs;
+        double mRadarObs;
+        double mObs;
+
+
+        GPS_INS aim_gps_ins;
+        bool bocheMode=false;
+        int bocheEnable=0;
+
+        double mfParkLat;
+        double mfParkLng;
+        double mfParkHeading;
+        int mnParkType;
+
+
+        double mLidarRotation;
+        double mLidarRangeUnit;
+
+        iv::GPSData location;		//当前车辆位置
+        boost::array<iv::ObstacleBasic, 64> obs_radar;//毫米波雷达的障碍物数据
+         boost::array<iv::ObstacleBasic, 64> obs_rear_radar;//houxiang毫米波雷达的障碍物数据
+        iv::ultrasonic_obs multrasonic_obs;
+        double mfGPSAcc = 0;
+
+//        iv::brain::decitionview mdecitionview;
+        iv::sysparam msysparam;
+        iv::roadmode_vel mroadmode_vel;
+        int vehGroupId;
+        double mfEPSOff = 0.0;
+        float socfDis=15;   //停障触发距离
+        float aocfDis=25;   //避障触发距离
+        float aocfTimes=5; //避障触发次数
+        float aobzDis=5;   //避障保障距离
+        /// traffice light : 0x90
+        int iTrafficeLightID = 0;       //红绿灯ID
+        int iTrafficeLightColor = 2;    //0x0: 红色
+        //0x1: 黄色
+        //0x2: 绿色
+        int iTrafficeLightTime = 0;     //红绿灯剩余时间
+        double iTrafficeLightLat = 0;
+        double iTrafficeLightLon = 0;
+
+        bool daocheMode=false;
+
+        //////////////////////////////////////////////////////
+
+        ///kkcai, 2020-01-03
+        QMutex mMutexMap;
+        /////////////////////////////////////
+
+        //mobileEye chuku
+
+        float vehSpeed_st=0;
+
+        bool useMobileEye = false;
+        bool m_bOutGarage=false;//车道线识别出库标志
+        float outGarageLong=10;
+        float waitGpsTimeWidth=5000;
+
+        int mnBocheComplete = 0;   //When boche comple set a value.
+
+        bool useObsPredict = false;
+
+    };
+    typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
+}
+#define ServiceCarStatus iv::CarStatusSingleton::get_mutable_instance()
+#endif // !_IV_COMMON_CAR_STATUS_

src/decition/common/common_sf/constants.h

@@ -1,43 +1,43 @@
-#pragma once
-#ifndef _IV_COMMON_CONSTANTS_
-#define _IV_COMMON_CONSTANTS_
-
-#include <common/car_status.h>
-namespace iv {
-
-
-    const std::uint8_t SPEED_MAX = 30;						//车速上限
-    const std::uint8_t BRAKING_PRESSURE_PERCENT_MAX = 50;	//刹车压力上限
-    const std::uint8_t ACCELERATE_PERCENT_MAX = 1;			//油门开合度上限
-
-    const std::uint8_t CONTROL_SPEED_TOLERANCE = 5;		//控制车速时容忍误差范围
-    const std::uint8_t CONTROL_WHEEL_ANGLE_TOLERANCE = 5;	//控制方向盘时容忍误差范围
-
-    const std::uint16_t WHEEL_ZERO = 0x1F4A;
-    //static iv::CarStatus CarStatus_(new iv::CarStatusBasic);
-
-    const float SPEED_RATIO = 0.08;		//实际车速和can中读出的车速比率
-
-    const float Veh_Width = 2.1;		//车宽
-    const float Veh_Lenth = 4.6;
-    const float Esr_Offset = 0;		//ESR偏移量
-    const float Esr_Y_Offset = 2.5;  //ESR纵向偏移量
-
-    const std::uint8_t THREAD_EXECUTECONTROL_MAXNUM = 1;	//decition_executer 执行线程最大数量
-
-
-    const double MaxValue = 1.7976931348623157E+308;  //     Represents the largest possible value of a System.Double. This field is constant.
-
-
-    const double MinValue = -1.7976931348623157E+308; //     Represents the smallest possible value of a System.Double. This field is constant.
-
-
-
-
-
-
-}
-
-
-
-#endif // !_IV_COMMON_CONSTANTS_
+#pragma once
+#ifndef _IV_COMMON_CONSTANTS_
+#define _IV_COMMON_CONSTANTS_
+
+#include <common/car_status.h>
+namespace iv {
+
+
+    const std::uint8_t SPEED_MAX = 30;						//车速上限
+    const std::uint8_t BRAKING_PRESSURE_PERCENT_MAX = 50;	//刹车压力上限
+    const std::uint8_t ACCELERATE_PERCENT_MAX = 1;			//油门开合度上限
+
+    const std::uint8_t CONTROL_SPEED_TOLERANCE = 5;		//控制车速时容忍误差范围
+    const std::uint8_t CONTROL_WHEEL_ANGLE_TOLERANCE = 5;	//控制方向盘时容忍误差范围
+
+    const std::uint16_t WHEEL_ZERO = 0x1F4A;
+    //static iv::CarStatus CarStatus_(new iv::CarStatusBasic);
+
+    const float SPEED_RATIO = 0.08;		//实际车速和can中读出的车速比率
+
+    const float Veh_Width = 2.1;		//车宽
+    const float Veh_Lenth = 4.6;
+    const float Esr_Offset = 0;		//ESR偏移量
+    const float Esr_Y_Offset = 2.5;  //ESR纵向偏移量
+
+    const std::uint8_t THREAD_EXECUTECONTROL_MAXNUM = 1;	//decition_executer 执行线程最大数量
+
+
+    const double MaxValue = 1.7976931348623157E+308;  //     Represents the largest possible value of a System.Double. This field is constant.
+
+
+    const double MinValue = -1.7976931348623157E+308; //     Represents the smallest possible value of a System.Double. This field is constant.
+
+
+
+
+
+
+}
+
+
+
+#endif // !_IV_COMMON_CONSTANTS_

src/decition/common/common_sf/gps_type.h

@@ -1,100 +1,100 @@
-#pragma once
-/*
-*GPS 惯导数据
-*/
-#ifndef _IV_COMMON_GPS_TYPE_
-#define _IV_COMMON_GPS_TYPE_
-
-#include <common/boost.h>
-namespace iv {
-    struct GPS_INS
-    {
-        int valid = 0xff;
-        int index = 0;	//gps点序号
-
-        double gps_lat = 0;//纬度
-        double gps_lng = 0;//经度
-
-        double gps_x = 0;
-        double gps_y = 0;
-        double gps_z = 0;
-
-        double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
-        double ins_pitch_angle = 0;	//俯仰角
-        double ins_heading_angle = 0;	//航向角
-
-        int ins_status = 0;	//惯导状态 4
-        int rtk_status = 0;	//rtk状态 6 -5 -3
-        int gps_satelites_num = 0;
-
-        //-----加速度--------------
-        double accel_x = 0;
-        double accel_y = 0;
-        double accel_z = 0;
-
-        //-------角速度------------
-        double ang_rate_x = 0;
-        double ang_rate_y = 0;
-        double ang_rate_z = 0;
-
-        //-----------方向速度--------------
-        double vel_N = 0;
-        double vel_E = 0;
-        double vel_D = 0;
-
-        int speed_mode = 0;
-        int mode2 = 0;
-        double speed = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
-
-        int roadMode;
-        int runMode;
-        int roadSum;
-        int roadOri;
-
-
-
-    };
-
-//    typedef std::shared_ptr<iv::GPS_INS> GPSData;
-    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
-         int v1 = 0, v2 = 0;
-         int roadMode = 0;
-         int obs_type=0;
-
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
-
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
-
-
-     };
-
-
-     class TrafficLight
-    {
-      public:
-         int leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
-         int leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
-
-         TrafficLight()
-        {
-            leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
-            leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
-        }
-
-     };
-
-
-
-
-}
-#endif // !_IV_COMMON_GPS_TYPE_
+#pragma once
+/*
+*GPS 惯导数据
+*/
+#ifndef _IV_COMMON_GPS_TYPE_
+#define _IV_COMMON_GPS_TYPE_
+
+#include <common/boost.h>
+namespace iv {
+    struct GPS_INS
+    {
+        int valid = 0xff;
+        int index = 0;	//gps点序号
+
+        double gps_lat = 0;//纬度
+        double gps_lng = 0;//经度
+
+        double gps_x = 0;
+        double gps_y = 0;
+        double gps_z = 0;
+
+        double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
+        double ins_pitch_angle = 0;	//俯仰角
+        double ins_heading_angle = 0;	//航向角
+
+        int ins_status = 0;	//惯导状态 4
+        int rtk_status = 0;	//rtk状态 6 -5 -3
+        int gps_satelites_num = 0;
+
+        //-----加速度--------------
+        double accel_x = 0;
+        double accel_y = 0;
+        double accel_z = 0;
+
+        //-------角速度------------
+        double ang_rate_x = 0;
+        double ang_rate_y = 0;
+        double ang_rate_z = 0;
+
+        //-----------方向速度--------------
+        double vel_N = 0;
+        double vel_E = 0;
+        double vel_D = 0;
+
+        int speed_mode = 0;
+        int mode2 = 0;
+        double speed = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        int roadMode;
+        int runMode;
+        int roadSum;
+        int roadOri;
+
+
+
+    };
+
+//    typedef std::shared_ptr<iv::GPS_INS> GPSData;
+    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
+     class Point2D
+    {
+      public:
+          double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+         int v1 = 0, v2 = 0;
+         int roadMode = 0;
+         int obs_type=0;
+
+         Point2D()
+        {
+            x = y = v1 = 0;
+        }
+
+         Point2D(double _x, double _y)
+        {
+            x = _x; y = _y;
+        }
+
+
+     };
+
+
+     class TrafficLight
+    {
+      public:
+         int leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+         int leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+
+         TrafficLight()
+        {
+            leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+            leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+        }
+
+     };
+
+
+
+
+}
+#endif // !_IV_COMMON_GPS_TYPE_

src/decition/common/common_sf/lidar.cpp

@@ -1,84 +1,84 @@
-#include "lidar.h"
-
-iv::Lidar::Lidar()
-{
-	Lidar_Grid = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-    Lidar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-
-    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> pertem(new std::vector<iv::Perception::PerceptionOutput>);
-    mper = pertem;
-}
-
-iv::Lidar::~Lidar()
-{
-}
-
-void iv::Lidar::copyfromlidar(iv::ObsLiDAR& obs)
-{
-    is_run = true;
-    if(mtx1.try_lock() == true)
-    {
-        Lidar_->clear();
-        Lidar_.swap(obs);
-        genggai1 = true;
-        mtx1.unlock();
-    }
-}
-void iv::Lidar::copylidarto(iv::ObsLiDAR& obs)
-{
-	if (genggai1 == true)
-	{
-        mtx1.lock();
-        obs->clear();
-        Lidar_.swap(obs);
-		genggai1 = false;
-		mtx1.unlock();
-	}
-}
-
-void iv::Lidar::copyfromlidarobs(iv::ObsLiDAR& obs)
-{
-    if(mtx2.try_lock() == true)
-    {
-        Lidar_Grid->clear();
-        Lidar_Grid.swap(obs);
-        genggai2 = true;
-        mtx2.unlock();
-    }
-}
-void iv::Lidar::copylidarobsto(iv::ObsLiDAR& obs)
-{
-	if (genggai2 == true)
-	{
-		mtx2.lock();
-		obs->clear();
-		Lidar_Grid.swap(obs);
-		genggai2 = false;
-		mtx2.unlock();
-	}
-}
-
-void iv::Lidar::copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > & per)
-{
-    mMutexper.lock();
-    mper.swap(per);
-    mMutexper.unlock();
-}
-
-void iv::Lidar::copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > & per)
-{
- //   int nSize = mper->size();
-    mMutexper.lock();
-//    per.swap(mper);
-    mper.swap(per);
-    mMutexper.unlock();
-//    if(nSize > 0)qDebug("have per");
-}
-
-
-bool iv::Lidar::did_lidar_ok()
-{
-    bool temp = is_run;
-    is_run = false;
-    return temp;
-}
+#include "lidar.h"
+
+iv::Lidar::Lidar()
+{
+	Lidar_Grid = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+    Lidar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+
+    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> pertem(new std::vector<iv::Perception::PerceptionOutput>);
+    mper = pertem;
+}
+
+iv::Lidar::~Lidar()
+{
+}
+
+void iv::Lidar::copyfromlidar(iv::ObsLiDAR& obs)
+{
+    is_run = true;
+    if(mtx1.try_lock() == true)
+    {
+        Lidar_->clear();
+        Lidar_.swap(obs);
+        genggai1 = true;
+        mtx1.unlock();
+    }
+}
+void iv::Lidar::copylidarto(iv::ObsLiDAR& obs)
+{
+	if (genggai1 == true)
+	{
+        mtx1.lock();
+        obs->clear();
+        Lidar_.swap(obs);
+		genggai1 = false;
+		mtx1.unlock();
+	}
+}
+
+void iv::Lidar::copyfromlidarobs(iv::ObsLiDAR& obs)
+{
+    if(mtx2.try_lock() == true)
+    {
+        Lidar_Grid->clear();
+        Lidar_Grid.swap(obs);
+        genggai2 = true;
+        mtx2.unlock();
+    }
+}
+void iv::Lidar::copylidarobsto(iv::ObsLiDAR& obs)
+{
+	if (genggai2 == true)
+	{
+		mtx2.lock();
+		obs->clear();
+		Lidar_Grid.swap(obs);
+		genggai2 = false;
+		mtx2.unlock();
+	}
+}
+
+void iv::Lidar::copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > & per)
+{
+    mMutexper.lock();
+    mper.swap(per);
+    mMutexper.unlock();
+}
+
+void iv::Lidar::copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > & per)
+{
+ //   int nSize = mper->size();
+    mMutexper.lock();
+//    per.swap(mper);
+    mper.swap(per);
+    mMutexper.unlock();
+//    if(nSize > 0)qDebug("have per");
+}
+
+
+bool iv::Lidar::did_lidar_ok()
+{
+    bool temp = is_run;
+    is_run = false;
+    return temp;
+}

src/decition/common/common_sf/lidar.h

@@ -1,39 +1,39 @@
-#pragma once
-#ifndef _IV_COMMON_LIDAR_
-#define _IV_COMMON_LIDAR_
-
-#include <common/boost.h>
-#include <boost/serialization/singleton.hpp>
-#include <common/obstacle_type.h>
-#include <common/car_status.h>
-
-#include <QMutex>
-
-#include "perception/perceptionoutput.h"
-
-namespace iv {
-	class Lidar : public boost::noncopyable {
-	public:
-		Lidar();
-		~Lidar();
-		bool genggai1 = false, genggai2 = false;
-        bool is_run = false;
-		boost::mutex mtx1, mtx2;
-		iv::ObsLiDAR Lidar_Grid;
-        iv::ObsLiDAR Lidar_;
-        QMutex mMutexper;
-        std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> mper;
-
-        void copyfromlidar(iv::ObsLiDAR& obs);
-        void copylidarto(iv::ObsLiDAR& obs);
-		void copyfromlidarobs(iv::ObsLiDAR& obs);
-        void copylidarobsto(iv::ObsLiDAR& obs);
-
-        void copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
-        void copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
-        bool did_lidar_ok();
-	};
-	typedef boost::serialization::singleton<iv::Lidar> LidarSingleton;
-}
-#define ServiceLidar iv::LidarSingleton::get_mutable_instance()
-#endif // !_IV_COMMON_LIDAR_
+#pragma once
+#ifndef _IV_COMMON_LIDAR_
+#define _IV_COMMON_LIDAR_
+
+#include <common/boost.h>
+#include <boost/serialization/singleton.hpp>
+#include <common/obstacle_type.h>
+#include <common/car_status.h>
+
+#include <QMutex>
+
+#include "perception/perceptionoutput.h"
+
+namespace iv {
+	class Lidar : public boost::noncopyable {
+	public:
+		Lidar();
+		~Lidar();
+		bool genggai1 = false, genggai2 = false;
+        bool is_run = false;
+		boost::mutex mtx1, mtx2;
+		iv::ObsLiDAR Lidar_Grid;
+        iv::ObsLiDAR Lidar_;
+        QMutex mMutexper;
+        std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> mper;
+
+        void copyfromlidar(iv::ObsLiDAR& obs);
+        void copylidarto(iv::ObsLiDAR& obs);
+		void copyfromlidarobs(iv::ObsLiDAR& obs);
+        void copylidarobsto(iv::ObsLiDAR& obs);
+
+        void copyfromlidarper(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
+        void copylidarperto(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> & per);
+        bool did_lidar_ok();
+	};
+	typedef boost::serialization::singleton<iv::Lidar> LidarSingleton;
+}
+#define ServiceLidar iv::LidarSingleton::get_mutable_instance()
+#endif // !_IV_COMMON_LIDAR_

src/decition/common/common_sf/logout.h

@@ -1,136 +1,136 @@
-/**
-* 用于输出log文件的类.
-*/
-
-
-#ifndef LOG_H      
-#define LOG_H      
-
-
-//log文件路径    
-#define LOG_FILE_NAME "log_319.txt"    
-
-//启用开关    
-#define LOG_ENABLE    
-
-#include <fstream>      
-#include <string>      
-#include <ctime>
-//#include <Windows.h>
-//#include <tchar.h>
-/*
-using namespace std;
-
-class CLog
-{
-public:
-	static void GetLogFilePath(CHAR* szPath)
-	{
-		GetModuleFileNameA(NULL, szPath, MAX_PATH);
-		memset(strrchr(szPath, _T('\\')), NULL, strlen(strrchr(szPath, _T('\\'))) * sizeof(CHAR));
-		strcat(szPath, "\\");
-		strcat(szPath, LOG_FILE_NAME);
-	}
-	//输出一个内容，可以是字符串(ascii)、整数、浮点数、布尔、枚举    
-	//格式为：[2011-11-11 11:11:11] aaaaaaa并换行    
-	template <class T>
-	static void WriteLog(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x << endl;
-		fout.close();
-	}
-
-	//输出2个内容，以等号连接。一般用于前面是一个变量的描述字符串，后面接这个变量的值    
-	template<class T1, class T2>
-	static void WriteLog2(T1 x1, T2 x2)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x1 << " = " << x2 << endl;
-		fout.close();
-	}
-
-	template<class T1, class T2, class T3, class T4, class T5, class T6 >
-	static void WriteLog6(T1 x1, T2 x2, T3 x3, T4 x4, T5 x5, T6 x6)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << x1 << "  " << x2 << "  "  
-								<< x3 << "  " << x4 << "  " 
-								<< x5 << "  " << x6 << endl;
-		fout.close();
-	}
-
-	//输出一行当前函数开始的标志,宏传入__FUNCTION__    
-	template <class T>
-	static void WriteFuncBegin(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << "    --------------------" << x << "  Begin--------------------" << endl;
-		fout.close();
-	}
-
-	//输出一行当前函数结束的标志，宏传入__FUNCTION__    
-	template <class T>
-	static void WriteFuncEnd(T x)
-	{
-		CHAR szPath[MAX_PATH] = { 0 };
-		GetLogFilePath(szPath);
-		ofstream fout(szPath, ios::app);
-		fout.seekp(ios::end);
-		fout << GetSystemTime() << "--------------------" << x << "  End  --------------------" << endl;
-		fout.close();
-	}
-
-
-private:
-	//获取本地时间，格式如"[2011-11-11 11:11:11] ";     
-	static string GetSystemTime()
-	{
-		time_t tNowTime;
-		time(&tNowTime);
-		tm* tLocalTime = localtime(&tNowTime);
-		char szTime[30] = { '\0' };
-		strftime(szTime, 30, "[%Y-%m-%d %H:%M:%S] ", tLocalTime);
-		string strTime = szTime;
-		return strTime;
-	}
-
-};
-
-#ifdef LOG_ENABLE    
-
-//用下面这些宏来使用本文件    
-#define LOG(x)              CLog::WriteLog(x);          //括号内可以是字符串(ascii)、整数、浮点数、bool等    
-#define LOG2(x1,x2)     CLog::WriteLog2(x1,x2);    
-#define LOG6(x1,x2,x3,x4,x5,x6)		CLog::WriteLog6(x1,x2,x3,x4,x5,x6);
-#define LOG_FUNC        LOG(__FUNCTION__)               //输出当前所在函数名    
-#define LOG_LINE        LOG(__LINE__)                       //输出当前行号    
-#define LOG_FUNC_BEGIN  CLog::WriteFuncBegin(__FUNCTION__);     //形式如：[时间]"------------FuncName  Begin------------"    
-#define LOG_FUNC_END     CLog::WriteFuncEnd(__FUNCTION__);      //形式如：[时间]"------------FuncName  End------------"    
-
-#else    
-
-#define LOG(x)                  
-#define LOG2(x1,x2)         
-#define LOG6(x1,x2,x3,x4,x5,x6)
-#define LOG_FUNC            
-#define LOG_LINE            
-#define LOG_FUNC_BEGIN      
-#define LOG_FUNC_END        
-
-#endif    
-*/
-#endif      
+/**
+* 用于输出log文件的类.
+*/
+
+
+#ifndef LOG_H      
+#define LOG_H      
+
+
+//log文件路径    
+#define LOG_FILE_NAME "log_319.txt"    
+
+//启用开关    
+#define LOG_ENABLE    
+
+#include <fstream>      
+#include <string>      
+#include <ctime>
+//#include <Windows.h>
+//#include <tchar.h>
+/*
+using namespace std;
+
+class CLog
+{
+public:
+	static void GetLogFilePath(CHAR* szPath)
+	{
+		GetModuleFileNameA(NULL, szPath, MAX_PATH);
+		memset(strrchr(szPath, _T('\\')), NULL, strlen(strrchr(szPath, _T('\\'))) * sizeof(CHAR));
+		strcat(szPath, "\\");
+		strcat(szPath, LOG_FILE_NAME);
+	}
+	//输出一个内容，可以是字符串(ascii)、整数、浮点数、布尔、枚举    
+	//格式为：[2011-11-11 11:11:11] aaaaaaa并换行    
+	template <class T>
+	static void WriteLog(T x)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << x << endl;
+		fout.close();
+	}
+
+	//输出2个内容，以等号连接。一般用于前面是一个变量的描述字符串，后面接这个变量的值    
+	template<class T1, class T2>
+	static void WriteLog2(T1 x1, T2 x2)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << x1 << " = " << x2 << endl;
+		fout.close();
+	}
+
+	template<class T1, class T2, class T3, class T4, class T5, class T6 >
+	static void WriteLog6(T1 x1, T2 x2, T3 x3, T4 x4, T5 x5, T6 x6)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << x1 << "  " << x2 << "  "  
+								<< x3 << "  " << x4 << "  " 
+								<< x5 << "  " << x6 << endl;
+		fout.close();
+	}
+
+	//输出一行当前函数开始的标志,宏传入__FUNCTION__    
+	template <class T>
+	static void WriteFuncBegin(T x)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << "    --------------------" << x << "  Begin--------------------" << endl;
+		fout.close();
+	}
+
+	//输出一行当前函数结束的标志，宏传入__FUNCTION__    
+	template <class T>
+	static void WriteFuncEnd(T x)
+	{
+		CHAR szPath[MAX_PATH] = { 0 };
+		GetLogFilePath(szPath);
+		ofstream fout(szPath, ios::app);
+		fout.seekp(ios::end);
+		fout << GetSystemTime() << "--------------------" << x << "  End  --------------------" << endl;
+		fout.close();
+	}
+
+
+private:
+	//获取本地时间，格式如"[2011-11-11 11:11:11] ";     
+	static string GetSystemTime()
+	{
+		time_t tNowTime;
+		time(&tNowTime);
+		tm* tLocalTime = localtime(&tNowTime);
+		char szTime[30] = { '\0' };
+		strftime(szTime, 30, "[%Y-%m-%d %H:%M:%S] ", tLocalTime);
+		string strTime = szTime;
+		return strTime;
+	}
+
+};
+
+#ifdef LOG_ENABLE    
+
+//用下面这些宏来使用本文件    
+#define LOG(x)              CLog::WriteLog(x);          //括号内可以是字符串(ascii)、整数、浮点数、bool等    
+#define LOG2(x1,x2)     CLog::WriteLog2(x1,x2);    
+#define LOG6(x1,x2,x3,x4,x5,x6)		CLog::WriteLog6(x1,x2,x3,x4,x5,x6);
+#define LOG_FUNC        LOG(__FUNCTION__)               //输出当前所在函数名    
+#define LOG_LINE        LOG(__LINE__)                       //输出当前行号    
+#define LOG_FUNC_BEGIN  CLog::WriteFuncBegin(__FUNCTION__);     //形式如：[时间]"------------FuncName  Begin------------"    
+#define LOG_FUNC_END     CLog::WriteFuncEnd(__FUNCTION__);      //形式如：[时间]"------------FuncName  End------------"    
+
+#else    
+
+#define LOG(x)                  
+#define LOG2(x1,x2)         
+#define LOG6(x1,x2,x3,x4,x5,x6)
+#define LOG_FUNC            
+#define LOG_LINE            
+#define LOG_FUNC_BEGIN      
+#define LOG_FUNC_END        
+
+#endif    
+*/
+#endif      

src/decition/common/common_sf/obstacle_type.h

@@ -1,69 +1,69 @@
-#pragma once
-#ifndef _IV_COMMON_OBSTACLE_TYPE_
-#define _IV_COMMON_OBSTACLE_TYPE_
-#include <vector>
-#include <common/boost.h>
-#include "fusionobjectarray.pb.h"
-/**
-*障碍物类型
-*/
-namespace iv {
-    const int grx = 250, gry = 500, centerx = 125, centery = 250;
-    const double gridwide = 0.2;
-    struct ObstacleBasic
-    {
-        bool valid;
-        float nomal_x;
-        float nomal_y;
-        float nomal_z;
-
-        float speed_relative;
-        float speed_x;
-        float speed_y;
-        float speed_z;
-
-        float high;
-        float low;
-
-        int esr_ID;
-    };
-
-    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsLiDAR;
-    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsRadar;
-    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsCamera;
-    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>>ObsRadarPointer;
-    typedef boost::shared_ptr<iv::fusion::fusionobjectarray> ObsFsuion;
-    struct Obs_grid
-    {
-        int  id;
-        double speed_x;
-        double speed_y;
-        double yaw;
-        int type;
-        double high;
-        double low;
-        double obshight;
-        int pointcount;
-        int ob;//障碍物属性，0地面 ，1路沿，2障碍物
-    };
-
-    typedef Obs_grid LidarGrid;
-    typedef Obs_grid* LidarGridPtr;
-
-    struct array_360
-    {
-        float x;
-        float y;
-        float z;
-    };
-
-    struct detect_info
-    {
-        int light;
-    };
-
-    typedef detect_info CameraInfo;
-    typedef detect_info* CameraInfoPtr;
-}
-
-#endif // !_IV_COMMON_OBSTACLE_TYPE_
+#pragma once
+#ifndef _IV_COMMON_OBSTACLE_TYPE_
+#define _IV_COMMON_OBSTACLE_TYPE_
+#include <vector>
+#include <common/boost.h>
+#include "fusionobjectarray.pb.h"
+/**
+*障碍物类型
+*/
+namespace iv {
+    const int grx = 250, gry = 500, centerx = 125, centery = 250;
+    const double gridwide = 0.2;
+    struct ObstacleBasic
+    {
+        bool valid;
+        float nomal_x;
+        float nomal_y;
+        float nomal_z;
+
+        float speed_relative;
+        float speed_x;
+        float speed_y;
+        float speed_z;
+
+        float high;
+        float low;
+
+        int esr_ID;
+    };
+
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsLiDAR;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsRadar;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>> ObsCamera;
+    typedef boost::shared_ptr<std::vector<iv::ObstacleBasic>>ObsRadarPointer;
+    typedef boost::shared_ptr<iv::fusion::fusionobjectarray> ObsFsuion;
+    struct Obs_grid
+    {
+        int  id;
+        double speed_x;
+        double speed_y;
+        double yaw;
+        int type;
+        double high;
+        double low;
+        double obshight;
+        int pointcount;
+        int ob;//障碍物属性，0地面 ，1路沿，2障碍物
+    };
+
+    typedef Obs_grid LidarGrid;
+    typedef Obs_grid* LidarGridPtr;
+
+    struct array_360
+    {
+        float x;
+        float y;
+        float z;
+    };
+
+    struct detect_info
+    {
+        int light;
+    };
+
+    typedef detect_info CameraInfo;
+    typedef detect_info* CameraInfoPtr;
+}
+
+#endif // !_IV_COMMON_OBSTACLE_TYPE_

src/decition/common/control/can.cpp

@@ -1,1266 +1,1266 @@
-#include <control/can.h>
-#include <control/control_status.h>
-#include <common/constants.h>
-#include <common/logout.h>
-#include <perception/sensor_radar.h>
-
-#include <QTime>
-
-#define VCI_USBCAN1		3
-#define VCI_USBCAN2		4
-#define VCI_USBCAN2A		4
-
-#define VCI_USBCAN_E_U 		20
-#define VCI_USBCAN_2E_U 	21
-
-#define VCI_ACCCODE_DEFAULT		0x00000000
-#define VCI_ACCMASK_DEFAULT		0xFFFFFFFF
-#define VCI_TIMER0_DEFAULT		0x00
-#define VCI_TIMER1_DEFAULT		0x1C
-#define VCI_FILTER_DEFAULT		1
-#define VCI_MODE_DEFAULT		0
-
-//函数调用返回状态值
-#define	STATUS_OK					1
-#define STATUS_ERR					0
-
-#define LOG_ENABLE
-#ifndef M_PI
-#define M_PI (3.1415926535897932384626433832795)
-#endif
-
-iv::control::CanUtil::CanUtil() {
-	obs_radar1 = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-	obs_radar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-	obs_radar_ui = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-#ifdef WIN32
-    m_hDLL = LoadLibrary(TEXT("ControlCAN.dll"));
-
-        VCI_OpenDevice = (LPVCI_OpenDevice)GetProcAddress(m_hDLL, "VCI_OpenDevice");//取得函数地址
-        VCI_CloseDevice = (LPVCI_CloseDevice)GetProcAddress(m_hDLL, "VCI_CloseDevice");
-        VCI_InitCAN = (LPVCI_InitCan)GetProcAddress(m_hDLL, "VCI_InitCAN");
-        VCI_ReadBoardInfo = (LPVCI_ReadBoardInfo)GetProcAddress(m_hDLL, "VCI_ReadBoardInfo");
-        VCI_ReadErrInfo = (LPVCI_ReadErrInfo)GetProcAddress(m_hDLL, "VCI_ReadErrInfo");
-        VCI_ReadCanStatus = (LPVCI_ReadCanStatus)GetProcAddress(m_hDLL, "VCI_ReadCANStatus");
-        VCI_GetReference = (LPVCI_GetReference)GetProcAddress(m_hDLL, "VCI_GetReference");
-        VCI_SetReference = (LPVCI_SetReference)GetProcAddress(m_hDLL, "VCI_SetReference");
-        VCI_GetReceiveNum = (LPVCI_GetReceiveNum)GetProcAddress(m_hDLL, "VCI_GetReceiveNum");
-        VCI_ClearBuffer = (LPVCI_ClearBuffer)GetProcAddress(m_hDLL, "VCI_ClearBuffer");
-        VCI_StartCAN = (LPVCI_StartCAN)GetProcAddress(m_hDLL, "VCI_StartCAN");
-        VCI_ResetCAN = (LPVCI_ResetCAN)GetProcAddress(m_hDLL, "VCI_ResetCAN");
-        VCI_Transmit = (LPVCI_Transmit)GetProcAddress(m_hDLL, "VCI_Transmit");
-        VCI_Receive = (LPVCI_Receive)GetProcAddress(m_hDLL, "VCI_Receive");
-        VCI_GetReference2 = (LPVCI_GetReference2)GetProcAddress(m_hDLL, "VCI_GetReference2");
-        VCI_SetReference2 = (LPVCI_SetReference2)GetProcAddress(m_hDLL, "VCI_SetReference2");
-        VCI_ResumeConfig = (LPVCI_ResumeConfig)GetProcAddress(m_hDLL, "VCI_ResumeConfig");
-        VCI_ConnectDevice = (LPVCI_ConnectDevice)GetProcAddress(m_hDLL, "VCI_ConnectDevice");
-        VCI_UsbDeviceReset = (LPVCI_UsbDeviceReset)GetProcAddress(m_hDLL, "VCI_UsbDeviceReset");
-#endif
-}
-iv::control::CanUtil::~CanUtil() {
-
-}
-
-void iv::control::CanUtil::openCanCard() {
-	VCI_INIT_CONFIG initconfig;
-	initconfig.AccCode = VCI_ACCCODE_DEFAULT;
-	initconfig.AccMask = VCI_ACCMASK_DEFAULT;
-	initconfig.Timing0 = VCI_TIMER0_DEFAULT;
-	initconfig.Timing1 = VCI_TIMER1_DEFAULT;
-	initconfig.Filter = VCI_FILTER_DEFAULT;
-	initconfig.Mode = VCI_MODE_DEFAULT;
-
-	int status = VCI_OpenDevice(m_devtype, m_devind, 0);		//打开设备
-    m_connect = -1;
-	if (status == 1)
-	{
-		int init = VCI_InitCAN(m_devtype, m_devind, m_cannum, &initconfig);	//初始化设备
-		if (init == 1)
-		{
-            if (VCI_StartCAN(m_devtype, m_devind, m_cannum) == 1) {
-                send_connect = true;
-                m_connect = 1;
-                thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this)));
-                thread_receive =  boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive, this)));
-            }
-            else {
-                throw 0;
-			}
-		}
-        else
-        {
-            throw 1;
-        }
-		//	打开2通道，供毫米波雷达读取数据用
-		init = VCI_InitCAN(m_devtype, m_devind, m_cannum_radar, &initconfig);	//初始化设备
-		if (init == 1)
-		{
-			if (VCI_StartCAN(m_devtype, m_devind, m_cannum_radar) == 1) {
-                m_connect = 1;
-                thread_radar_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_radar, this)));	//启动毫米波雷达接收线程5
-
-            }
-			else {
-                throw 2;
-			}
-			
-		}
-		else
-		{
-            throw 3;
-		}
-	}
-	else if(status == 0) {
-        throw 4;
-	}
-	else if (status == -1) {
-        throw 5;
-	}
-
-#ifdef USE_MOBILEYE
-    status = VCI_OpenDevice(m_devtype, m_devindmob, 0);		//打开设备
-    if (status == 1)
-    {
-        int init = VCI_InitCAN(m_devtype, m_devindmob, m_cannum, &initconfig);	//初始化设备
-        if (init == 1)
-        {
-            if (VCI_StartCAN(m_devtype, m_devindmob, m_cannum) == 1) {
-                m_connect = 1;
-                thread_mobileye_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_mobileye, this)));
-             }
-            else {
-                throw 0;
-            }
-        }
-        else
-        {
-            throw 1;
-        }
-    }
-    else if(status == 0) {
-        throw 4;
-    }
-    else if (status == -1) {
-        throw 5;
-    }
-
-#endif
-}
-
-void iv::control::CanUtil::closeCanCard() {
-    if(m_connect != 0)
-    {
-        thread_receive->interrupt();
-        thread_receive->join();
-        thread_send->interrupt();
-        thread_send->join();
-        thread_receive->interrupt();
-        thread_receive->join();
-        unsigned int rec = VCI_CloseDevice(m_devtype, m_devind);
-#ifdef USE_MOBILEYE
-        rec = VCI_CloseDevice(m_devtype, m_devindmob);
-#endif
-         std::cout << "close can:" << rec << std::endl;
-    }
-}
-
-void iv::control::CanUtil::receive() {
-    while (m_connect)
-    {
-        VCI_CAN_OBJ receivedata[2500];
-        int res = VCI_Receive(m_devtype, m_devind, m_cannum, receivedata, 2500, 10);
-        if (res > 0)	//未能从can卡中读到数
-        {
-   //         std::cout<<"reee"<<std::endl;
-            int i;
-            for(i=0;i<res;i++)
-            {
-                if(receivedata[i].ID == 0x282)
-                {
-                    int i;
-                    unsigned int xvalue[8];
-                    float fvalue[8];
-                    unsigned int x;
-                    for(i=0;i<8;i++)
-                    {
-                        memcpy(&x,&receivedata[i].Data[0],4);
-                        x = (x<<((7-i)*4))>>28;
-                        xvalue[i] = x;
-                        fvalue[i] = x;
-                        if(x == 0)fvalue[i] = -1;
-                        else fvalue[i] = (fvalue[i] - 1.0)*0.2;
-                    }
-                    ServiceCarStatus.multrasonic_obs.mfront_leftmid = fvalue[0];
-                    ServiceCarStatus.multrasonic_obs.mfront_left = fvalue[1];
-                    ServiceCarStatus.multrasonic_obs.mfront_right = fvalue[2];
-                    ServiceCarStatus.multrasonic_obs.mfront_rightmid = fvalue[3];
-                    ServiceCarStatus.multrasonic_obs.mrear_leftmid = fvalue[4];
-                    ServiceCarStatus.multrasonic_obs.mrear_left = fvalue[5];
-                    ServiceCarStatus.multrasonic_obs.mrear_right = fvalue[6];
-                    ServiceCarStatus.multrasonic_obs.mrear_rightmid = fvalue[7];
-
-//                    std::cout<<"rec oooo value is back "<<(int)(receivedata[i].Data[3])<<" head "<<(int)receivedata[i].Data[1]<<std::endl;
-                }
-            }
-        }
-        else
-        {
-            usleep(1000);
-        }
-    }
-}
-
-void iv::control::CanUtil::initial()
-{
-    ServiceControlStatus.command.byte[0] = 0xEB;
-
-    ServiceControlStatus.command.byte[1] = 0;
-
-    ServiceControlStatus.command.byte[2] = 0;
-
-    ServiceControlStatus.command.byte[3] = 0;
-
-    ServiceControlStatus.command.byte[4] = 0;
-
-    ServiceControlStatus.command.byte[5] = 0;
-
-    ServiceControlStatus.command.byte[6] = 0;
-
-    ServiceControlStatus.command.byte[7] = 0;
-}
-
-void iv::control::CanUtil::pack_command()
-{
-    ServiceControlStatus.command.bit.heartbreak++;
-
-    if (ServiceControlStatus.command.bit.heartbreak >= 256)
-        ServiceControlStatus.command.bit.heartbreak = 0;
-
-    ServiceControlStatus.command.byte[7] = (ServiceControlStatus.command.byte[1] + ServiceControlStatus.command.byte[2]
-        + ServiceControlStatus.command.byte[3] + ServiceControlStatus.command.byte[4]
-        + ServiceControlStatus.command.byte[5] + ServiceControlStatus.command.byte[6]) % 256;
-}
-
-void iv::control::CanUtil::send()
-{
-    VCI_CAN_OBJ Command_data;
-
-        QTime time1;
-
-        int LastTimeSend1;
-
-        time1.start();
-
-        LastTimeSend1 = time1.elapsed();
-
-        int Send1Int = 10;  //100ms
-
-        Command_data.SendType = m_sendtype;
-        Command_data.ExternFlag = m_frameflag;
-        Command_data.RemoteFlag = m_frameformat;
-
-        Command_data.ID = ServiceControlStatus.command_ID;
-        Command_data.DataLen = 8;
-
-        initial();
-
-        while (true)
-        {
-            int print_time = time1.elapsed() - LastTimeSend1;
-            if (print_time >= Send1Int)
-            {
-                LastTimeSend1 = time1.elapsed();
-
-                pack_command();
-
-                memcpy(Command_data.Data, ServiceControlStatus.command.byte, 8);
-
-                VCI_Transmit(m_devtype, m_devind, m_cannum, &Command_data, 1);
-
-                /*for (int c = 0; c < 8; c++)
-                ODS("%x  ", Command_data.Data[c]);
-
-                ODS("\n");*/
-
-                //ODS("    %d  \n", print_time);
-            }
-        }
-}
-
-/******************************************************mobileye AWS display*****************************************************
- *  message ID:0x700
- *  member:                 type            physical unit       range                   note
- *  dusk_time:              double                /              0,1
- *  night_time              double                /              0,1
- *******************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x700(VCI_CAN_OBJ receivedata)
-{
-    ServiceCarStatus.aws_display.dusk_time = (receivedata.Data[0] >> 3) & 0x1;
-    ServiceCarStatus.aws_display.night_time = (receivedata.Data[0] >> 4) & 0x1;
-    ServiceCarStatus.aws_display.lanes_on = receivedata.Data[4] & 0x1;
-    //std::cout << "lanes_on:" << ServiceCarStatus.aws_display.lanes_on << std::endl;
-}
-
-/******************************************************mobileye lane***********************************************************
- *  message ID:0x737
- *  member:                 type            physical unit       range                   note
- *  curvature:              double             1/meter      [-0.12, 0.12]       "a" in the equation:y = ax^2+bx+c
- *  heading:                double                /          [-1.0, 1.0]        "b" in the equation:y = ax^2+bx+c
- *  construction area:      bool                  /               /                       /
- *  pitch angle:            double             radians      [-0.05, 0.05]      pitch of the host vehicle(derived from lanes analysis)
- *  yaw angle:              double             radians            /            yaw of the host vehicle(derived from lanes analysis)
- *  right_LDW:              bit/bool              /              0,1           0 stands for unavailable, 1 for available
- *  left_LDW:               bit/bool              /              0,1           0 stands for unavailable, 1 for available
- *******************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x737(VCI_CAN_OBJ receivedata)
-{
-    //lane curvature
-    int32_t tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
-    tmp <<= 16;
-    tmp >>= 16;
-    ServiceCarStatus.Lane.curvature = tmp * 3.81 * 1e-6;
-    //std::cout<<"curvature:"<<ServiceCarStatus.Lane.curvature<<std::endl;
-
-    tmp = 0;
-
-    //lane heading
-    tmp = ((receivedata.Data[3] & 0xf) << 8) | receivedata.Data[2];
-    tmp <<= 20;
-    tmp >>= 20;
-    ServiceCarStatus.Lane.heading = tmp * 0.0005;
-
-    tmp = 0;
-
-    //construction area
-    ServiceCarStatus.Lane.is_ca = (receivedata.Data[3] >> 4) & 0x1;
-
-    //left_LDW
-    ServiceCarStatus.Lane.is_left_LDW_available = (receivedata.Data[3] >> 5) & 0x1;
-
-    //right_LDW
-    ServiceCarStatus.Lane.is_right_LDW_available = (receivedata.Data[3] >> 6) & 0x1;
-
-    //yaw
-    tmp = (receivedata.Data[5] >> 8) | receivedata.Data[4];
-    tmp <<= 16;
-    tmp >>= 16;
-    //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0;
-    ServiceCarStatus.Lane.yaw = tmp * 0.000977;
-    std::cout << "yaw:"<< ServiceCarStatus.Lane.yaw<< std::endl;
-
-    tmp = 0;
-
-    //pitch
-    tmp = (receivedata.Data[7] >> 8) | receivedata.Data[6];
-    tmp <<= 16;
-    tmp >>= 16;
-    //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0 / 512;
-    ServiceCarStatus.Lane.yaw = tmp * 0.000002;
-    std::cout << "pitch:" << ServiceCarStatus.Lane.pitch << std::endl;
-}
-
-/****************************************************  mobileye obstacle status  ****************************************************************************
- *  message ID:0x738
- *  member:                  type           physical unit       range               note
- *  num_obstacles            int                   /           [0:255]                /
- *  timestamp                int                   ms          [0:255]          only the lowest 8 bits of the timestamp is given
- *  app_version:             int                   /           [0:255]          vesion info consists of X.Y.Z.W
- *  active_version           int/2bits             /            [0:3]           index of the active section of app_version signal
- *  left_close_rang_cut_in   bool                  /             0,1            0 false, 1 true
- *  right_close_rang_cut_in  bool                  /             0,1            0 false, 1 true
- *  go                       enum                  /           0,1...15         0 stop, 1 go, 2 undecided, 3 driver decisions is required, 4-14 unused, 15 not calculated
- *  protocol version         uchar/8bit            /          0x00...0xff             /
- *  is_close_car             bool                  /             0,1            0 no close, 1 close car exists
- *  failsafe                 int/4bit              /            [0:7]           failsafe situation, 0000 no failsafe, 0001 low sun, 0010 blur image, 0100 1000 unused
- *************************************************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x738(VCI_CAN_OBJ receivedata)
-{
-    //num_obstacles
-    ServiceCarStatus.obstacleStatus.num_obstacles = receivedata.Data[0];
-    //std::cout << ServiceCarStatus.obstacleStatus.num_obstacles<< std::endl;
-
-
-    //timestamp
-    ServiceCarStatus.obstacleStatus.timestamp = receivedata.Data[1];
-
-    //app version
-    ServiceCarStatus.obstacleStatus.app_version = receivedata.Data[2];
-
-    //active version
-    ServiceCarStatus.obstacleStatus.active_version = receivedata.Data[3] & 0x3;
-
-    //is left close rang cut in
-    ServiceCarStatus.obstacleStatus.is_left_close_rang_cut_in = (receivedata.Data[3] >> 2) & 0x1;
-
-    //is right close rang cut in
-    ServiceCarStatus.obstacleStatus.is_right_close_rang_cut_in = (receivedata.Data[3] >> 3) & 0x1;
-
-    //go
-    ServiceCarStatus.obstacleStatus.go = receivedata.Data[3] >> 4;
-
-    //protocol version
-    ServiceCarStatus.obstacleStatus.protocol_version = receivedata.Data[4];
-
-    //close car
-    ServiceCarStatus.obstacleStatus.is_close_car = receivedata.Data[5] & 0x1;
-
-    //failsafe
-    ServiceCarStatus.obstacleStatus.failsafe = (receivedata.Data[5] >> 1) & 0xf;
-}
-
-/****************************************************  mobileye obstacle data a  ****************************************************************************
- *  message ID:0x739
- *  member:                  type           physical unit       range               note
- *  obstacle_ID              int                   /           [0:63]           new obstacles are given the last used free ID
- *  obstacle_pos_x           double                m           [0,250]          longtitude position of the obstacle relative to the ref point
- *  obstacle_pos_y           double                m       [-31.93,31.93]       lateral position of the obstacle, error relative to pos_x  below 10% or 2meters
- *  blinker_info             int/enum              /            [0:4]           0 unavailable, 1 off, 2 left, 3 right, 4 both   indicated blinkers status
- *  cut_in_and_out           int                   /            [0:4]           0 undefined, 1 in_host_lane, 2 out_host_lane, 3 cut_in, 4 cut_out
- *                                                                              cut_in: target entering the host lane, cut_out: exiting  but not distinguish between sides
- *  obstacle_rel_vel_x       double               m/s      [-127.93,127.93]     relative longtitude velocity of the obstacle
- *  obstacle_status          int/enum              /            [0:6]           0 undefined, 1 standing(never moved, back lights on), 2 stopped(moveable), 3 moving
- *                                                                              4 oncoming,  5 parked(never moved, back lights off), 6 unused
- *  is_obstacle_brake_lights bool                  /             0,1            0 object's brake light off or not identified, 1 object's brake light on
- *  obstacle_valid           int/enum              /            [1:2]           1 new valid(detected this frame), 2 older valid
- *************************************************************************************************************************************************************/
-obstacle_data_a iv::control::CanUtil::impl_0x739(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-    obstacle_data_a obs_tmp;
-
-    //obstacle id
-    obs_tmp.obstacle_ID = receivedata.Data[0];
-    //ServiceCarStatus.obstacleStatusA.obstacle_ID = receivedata.Data[0];
-
-    //obstacle pos x
-    tmp = ((receivedata.Data[2] & 0xf) << 8) | receivedata.Data[1];
-    obs_tmp.obstacle_pos_x = tmp * 0.0625;
-    //ServiceCarStatus.obstacleStatusA.obstacle_pos_x = tmp * 0.0625;
-
-    tmp = 0;
-
-    //obstacle pos y
-    tmp = ((receivedata.Data[4] & 0x3) << 8) | receivedata.Data[3];
-    obs_tmp.obstacle_pos_y = tmp * 0.0625;
-    //ServiceCarStatus.obstacleStatusA.obstacle_pos_y = tmp * 0.0625;
-
-    tmp = 0;
-    //blinker info
-    obs_tmp.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
-    //ServiceCarStatus.obstacleStatusA.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
-
-    //cut in and out
-    obs_tmp.cut_in_and_out = receivedata.Data[4] >> 5;
-    //ServiceCarStatus.obstacleStatusA.cut_in_and_out = receivedata.Data[4] >> 5;
-
-    //obstacle rel vel x
-    tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
-    obs_tmp.obstacle_rel_vel_x = tmp * 0.0625;
-    //ServiceCarStatus.obstacleStatusA.obstacle_rel_vel_x = tmp * 0.0625;
-
-    tmp = 0;
-
-    //obstacle type
-    obs_tmp.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
-    //ServiceCarStatus.obstacleStatusA.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
-
-    //obstacle status
-    obs_tmp.obstacle_status = receivedata.Data[7] & 0x7;
-    //ServiceCarStatus.obstacleStatusA.obstacle_status = receivedata.Data[7] & 0x7;
-
-    //obstacle brake lights
-    obs_tmp.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
-    //ServiceCarStatus.obstacleStatusA.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
-
-    //obstacle valid
-    obs_tmp.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
-    //ServiceCarStatus.obstacleStatusA.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
-    return obs_tmp;
-}
-
-/****************************************************  mobileye obstacle data b  ****************************************************************************
- *  message ID:0x73a
- *  member:                  type           physical unit       range               note
- *  obstacle_length          double                m           [0,31]           length of the obstacle(longitude axis)
- *  obstacle_width           double                m           [0,12.5]         width of  the obstacle(lateral axis)
- *  obstacle_age             int                   /           [0:255]          value starts at 1 when it is first detected, increaments in i each frame
- *  obstacle_lane            int/enum              /           [0:3]            0 not assigned, 1 ego lane, 2 next lane(left or right) or next next lane,
- *                                                                              3 invalid signal
- *  is_cipv_flag             int/enum              /            [0:1]           0 not cipv, 1 cipv(closest in path vehicle)
- *  radar_pos_x              double                m           [0,250]          longtitude postion of the primary radar target matched to the vision target, dist=relative to ref point
- *                                                                              if no radar target is matched, value = 0xfff
- *  radar_vel_x              double               m/s       [-127.93,127.93]    longitude velocity of the radar target matched to the vision targets, if no radar target is matched value=0xfff
- *                                                                              4 oncoming,  5 parked(never moved, back lights off), 6 unused
- *  radar_match_confidence   int                   /            [0:5]           0 no match, 1 multi match: radar doesn't describe well, 2-4 vision-radar match:with bounded error between
- *                                                                              vision and radar measurements higher->smaller error, 5 high confidence match
- *  matched_radar_id         int                   /           [0:127]          ID of Primary radar target matched to the vision target if applicable
- *************************************************************************************************************************************************************/
-obstacle_data_b iv::control::CanUtil::impl_0x73a(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-    obstacle_data_b obs_tmp;
-
-    //obstacle length
-    tmp = receivedata.Data[0];
-    obs_tmp.obstacle_length = tmp * 0.5;
-    //ServiceCarStatus.obstacleStatusB.obstacle_length = tmp * 0.5;
-
-    tmp = 0;
-
-    //obstacle width
-    tmp = receivedata.Data[1];
-    obs_tmp.obstacle_width = tmp * 0.05;
-    //ServiceCarStatus.obstacleStatusB.obstacle_width = tmp * 0.05;
-
-    tmp = 0;
-
-    //obstacle age
-    obs_tmp.obstacle_age = receivedata.Data[2];
-    //ServiceCarStatus.obstacleStatusB.obstacle_age = receivedata.Data[2];
-
-    //obstacle lane
-    obs_tmp.obstacle_lane = receivedata.Data[3] & 0x3;
-    //ServiceCarStatus.obstacleStatusB.obstacle_lane = receivedata.Data[3] & 0x3;
-
-    //is cipv flag
-    obs_tmp.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
-    //ServiceCarStatus.obstacleStatusB.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
-
-    //radar pos x
-    tmp = (receivedata.Data[4] << 4) | (receivedata.Data[3] >> 4);
-    obs_tmp.radar_pos_x = tmp * 0.0625;
-    //ServiceCarStatus.obstacleStatusB.radar_pos_x = tmp * 0.0625;
-
-    tmp = 0;
-
-    //radar vel x
-    tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
-    tmp <<= 20;
-    tmp >>= 20;
-    obs_tmp.radar_vel_x = tmp * 0.0625;
-    //ServiceCarStatus.obstacleStatusB.radar_vel_x = tmp * 0.0625;
-
-    tmp = 0;
-
-    //radar match confidence
-    obs_tmp.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
-    //ServiceCarStatus.obstacleStatusB.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
-
-    //matched radar ID
-    obs_tmp.matched_radar_id = receivedata.Data[7] & 0x7f;
-    //ServiceCarStatus.obstacleStatusB.matched_radar_id = receivedata.Data[7] & 0x7f;
-
-    return obs_tmp;
-}
-
-/****************************************************  mobileye obstacle data c  ****************************************************************************
- *  message ID:0x73b
- *  member:                  type           physical unit       range               note
- *  obstacle_angle_rate      double           degree/s     [-327.68, 327.68]    Angle rate of Center of Obstacle, negative->moved to left
- *  obstcale_scale_change    double            pix/s       [-6.5532, 6.5532]    /
- *  object_accel_x           double            m/s2         [-14.97, 14.97]     longtitude acceleration of the object
- *  obstacle_replaced        bool                /               0,1            0 not replaced in this frame, 1 replace in this frame
- *  obstacle_angle           double           degree        [-327.68,327.68]    Angle to Center of Obstacle in degrees, 0 indicates that the obstacle is in
- *                                                                              exactly in front of us, a positive angle indicates that theobstacle is to the right
- *************************************************************************************************************************************************************/
-obstacle_data_c iv::control::CanUtil::impl_0x73b(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-    obstacle_data_c obs_tmp;
-
-    //obstacle angle rate
-    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
-    tmp <<= 16;
-    tmp >>= 16;
-    obs_tmp.obstacle_angle_rate = tmp * 0.01;
-    //ServiceCarStatus.obstacleStatusC.obstacle_angle_rate = tmp * 0.01;
-
-    tmp = 0;
-
-    //obstacle scale change
-    tmp = (receivedata.Data[3] << 8) | receivedata.Data[2];
-    tmp <<= 16;
-    tmp >>= 16;
-    obs_tmp.obstacle_scale_change = tmp * 0.0002;
-    //ServiceCarStatus.obstacleStatusC.obstacle_scale_change = tmp * 0.0002;
-
-    tmp = 0;
-
-    //object accel x
-    tmp = ((receivedata.Data[5] & 0x3) << 8) | receivedata.Data[4];
-    tmp <<= 22;
-    tmp >>= 22;
-    obs_tmp.object_accel_x = tmp * 0.03;
-    //ServiceCarStatus.obstacleStatusC.object_accel_x = tmp * 0.03;
-
-    tmp = 0;
-
-    //obstacle replaced
-    obs_tmp.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
-    //ServiceCarStatus.obstacleStatusC.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
-
-    //obstacle angle
-    tmp = (receivedata.Data[7] << 8) | receivedata.Data[6];
-    tmp <<= 16;
-    tmp >>= 16;
-    obs_tmp.obstacle_angle = tmp * 0.01;
-    //ServiceCarStatus.obstacleStatusC.obstacle_angle = tmp * 0.01;
-
-    return obs_tmp;
-}
-
-/****************************************************  mobileye aftermarket lane  ****************************************************************************
- *  message ID:0x669
- *  member:                  type           physical unit       range               note
- *  lane_conf_left           int/2bit              /            [0:3]            0 lowest, 3 highest
- *  is_ldw_availablity_left  bool                  /             0,1             lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
- *  lane_type_left           int/enum              /            [0:6]            0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
- *  dist_to_lane_l           double                m           [-40:40]          "c" in the equation:y = ax^2+bx+c
- *  lane_conf_right          int/2bit              /            [0:3]            0 lowest, 3 highest
- *  is_ldw_availablity_right  bool                  /             0,1             lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
- *  lane_type_right          int/enum              /            [0:6]            0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
- *  dist_to_lane_r           double                m           [-40:40]          "c" in the equation:y = ax^2+bx+c
- *************************************************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x669(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp;
-    //lane confidence left
-    tmp = receivedata.Data[0] & 0x3;
-    tmp <<= 30;
-    tmp >>= 30;
-    ServiceCarStatus.aftermarketLane.lane_conf_left = tmp;
-    //std::cout << "left confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_left << std::endl;
-
-    tmp = 0;
-
-    //is LDW availability left
-    ServiceCarStatus.aftermarketLane.is_ldw_availablility_left = (receivedata.Data[0] >> 2) & 0x1;
-
-    //lane type left
-    tmp = (receivedata.Data[0] >> 4) & 0xf;
-    tmp <<= 28;
-    tmp >>= 28;
-    ServiceCarStatus.aftermarketLane.lane_type_left = tmp;
-    std::cout << "lane_type_left: " << ServiceCarStatus.aftermarketLane.lane_type_left << std::endl;
-
-    tmp = 0;
-
-    //dist to left lane
-
-    tmp = (receivedata.Data[2] << 4) | ((receivedata.Data[1] >> 4) & 0xf);
-    tmp <<= 20;
-    tmp >>= 20;
-    ServiceCarStatus.aftermarketLane.dist_to_lane_l = tmp * 0.02;
-    std::cout << "dist_to_lane_l:" << ServiceCarStatus.aftermarketLane.dist_to_lane_l << std::endl;
-
-    tmp = 0;
-
-    //lane confidence right
-    tmp = receivedata.Data[5] & 0x3;
-    tmp <<= 30;
-    tmp >>= 30;
-    ServiceCarStatus.aftermarketLane.lane_conf_right = tmp;
-    //std::cout << "right confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_right << std::endl;
-
-    tmp = 0;
-
-    //is LDW availability right
-    ServiceCarStatus.aftermarketLane.is_ldw_availablility_right = (receivedata.Data[5] >> 2) & 0x1;
-
-    //lane type right
-    tmp = (receivedata.Data[5] >> 4) & 0xf;
-    tmp <<= 28;
-    tmp >>= 28;
-    ServiceCarStatus.aftermarketLane.lane_type_right = tmp;
-    std::cout<<"lane_type_right: " << ServiceCarStatus.aftermarketLane.lane_type_right << std::endl;
-
-    tmp = 0;
-
-    //dist to right lane
-    tmp = (receivedata.Data[7] << 4) | ((receivedata.Data[6] >> 4) & 0xf);
-    tmp <<= 20;
-    tmp >>= 20;
-    ServiceCarStatus.aftermarketLane.dist_to_lane_r = tmp * 0.02;
-    std::cout << "dist_to_lane_r:" << ServiceCarStatus.aftermarketLane.dist_to_lane_r << std::endl;
-
-    tmp = 0;
-}
-
-/****************************************************  mobileye LKA left lane a ****************************************************************************
- *  message ID:0x766
- *  member:                  type           physical unit       range               note
- *  lane_type                enum                  /            [0:6]            0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
- *  quality                  enum                  /            [0:3]            0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
- *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
- *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
- *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
- *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
- *  width_left_marking       double                m           [0, 2.5]          left lane marking width
- *************************************************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x766(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-
-    //lane type
-    ServiceCarStatus.LKAleftLaneA.lane_type = receivedata.Data[0] & 0xf;
-
-    //quality
-    ServiceCarStatus.LKAleftLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
-
-    //model degree
-    ServiceCarStatus.LKAleftLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
-
-    //position
-    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
-    tmp <<= 16;
-    tmp >>= 16;
-    ServiceCarStatus.LKAleftLaneA.position = tmp / 256.0;
-    //ServiceCarStatus.LKAleftLaneA.position = tmp * 0.003906;
-
-    tmp = 0;
-
-    //curvature
-    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
-    ServiceCarStatus.LKAleftLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
-    //ServiceCarStatus.LKAleftLaneA.curvature = tmp * 0.000001 + -0.031999;
-
-    tmp = 0;
-
-    //curvature derivative
-    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
-    ServiceCarStatus.LKAleftLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
-    //ServiceCarStatus.LKAleftLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
-
-    tmp = 0;
-
-    //width left marking
-    tmp = receivedata.Data[7];
-    ServiceCarStatus.LKAleftLaneA.width_left_marking = tmp * 0.01;
-
-}
-
-/****************************************************  mobileye LKA left lane b ****************************************************************************
- *  message ID:0x767
- *  member:                  type           physical unit       range               note
- *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
- *  view_range               double                m         [0, 127.996]        physical view range of lane mark
- *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
- *************************************************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x767(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-
-    //heading angle
-    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
-    ServiceCarStatus.LKAleftLaneB.heading_angle = (tmp - 0x7fff) / 1024;
-    //ServiceCarStatus.LKAleftLaneB.heading_angle = tmp * 0.000977 + -31.999023;
-
-    tmp = 0;
-
-    //view range
-    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
-    ServiceCarStatus.LKAleftLaneB.view_range = tmp / 256.0;
-    //ServiceCarStatus.LKAleftLaneB.view_range = tmp * 0.003906;
-
-    tmp = 0;
-
-    //view range availability
-    ServiceCarStatus.LKAleftLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
-
-}
-
-/****************************************************  mobileye LKA right lane a ****************************************************************************
- *  message ID:0x768
- *  member:                  type           physical unit       range               note
- *  lane_type                enum                  /            [0:6]            0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
- *  quality                  enum                  /            [0:3]            0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
- *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
- *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
- *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
- *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
- *  width_right_marking      double                m           [0, 2.5]          right lane marking width
- *************************************************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x768(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-
-    //lane type
-    ServiceCarStatus.LKArightLaneA.lane_type = receivedata.Data[0] & 0xf;
-
-    //quality
-    ServiceCarStatus.LKArightLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
-
-    //model degree
-    ServiceCarStatus.LKArightLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
-
-    //position
-    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
-    tmp <<= 16;
-    tmp >>= 16;
-    ServiceCarStatus.LKArightLaneA.position = tmp / 256.0;
-    //ServiceCarStatus.LKArightLaneA.position = tmp * 0.003906;
-
-    tmp = 0;
-
-    //curvature
-    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
-    ServiceCarStatus.LKArightLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
-    //ServiceCarStatus.LKArightLaneA.curvature = tmp * 0.000001 + -0.031999;
-
-    tmp = 0;
-
-    //curvature derivative
-    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
-    ServiceCarStatus.LKArightLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
-    //ServiceCarStatus.LKArightLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
-
-    tmp = 0;
-
-    //width left marking
-    tmp = receivedata.Data[7];
-    ServiceCarStatus.LKArightLaneA.width_left_marking = tmp * 0.01;
-}
-
-/****************************************************  mobileye LKA right lane b ****************************************************************************
- *  message ID:0x767
- *  member:                  type           physical unit       range               note
- *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
- *  view_range               double                m         [0, 127.996]        physical view range of lane mark
- *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
- *************************************************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x769(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-
-    //heading angle
-    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
-    ServiceCarStatus.LKArightLaneB.heading_angle = (tmp - 0x7fff) / 1024;
-    //ServiceCarStatus.LKArightLaneB.heading_angle = tmp * 0.000977 + -31.999023;
-
-    tmp = 0;
-
-    //view range
-    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
-    ServiceCarStatus.LKArightLaneB.view_range = tmp / 256.0;
-    //ServiceCarStatus.LKArightLaneB.view_range = tmp * 0.003906;
-
-    tmp = 0;
-
-    //view range availability
-    ServiceCarStatus.LKArightLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
-
-}
-
-/****************************************************  mobileye next lane a **********************************************************************************
- *  message ID:0x76c
- *  member:                  type           physical unit       range               note
- *  lane_type                enum                  /            [1:2]            1 solid, 2 undecided
- *  quality                    /                   /              /              not yet implemented for next lanes
- *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
- *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
- *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
- *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
- *  lane_mark_widt           double                m           [0, 2.5]          lane marking width
- *************************************************************************************************************************************************************/
-next_lane_a iv::control::CanUtil::impl_0x76c(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-    next_lane_a nextLane_tmp;
-
-    //lane type
-    nextLane_tmp.lane_type = receivedata.Data[0] & 0xf;
-    //ServiceCarStatus.nextLaneA.lane_type = receivedata.Data[0] & 0xf;
-
-    //quality
-    nextLane_tmp.quality = (receivedata.Data[0] >> 4) & 0x3;
-    //ServiceCarStatus.nextLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
-
-    //model degree
-    nextLane_tmp.model_degree = (receivedata.Data[0] >> 6) & 0x3;
-    //ServiceCarStatus.nextLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
-
-    //position
-    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
-    tmp <<= 16;
-    tmp >>= 16;
-    nextLane_tmp.position = tmp * 0.003906;
-    //ServiceCarStatus.nextLaneA.position = tmp / 256.0;
-    //ServiceCarStatus.nextLaneA.position = tmp * 0.003906;
-
-    tmp = 0;
-
-    //curvature
-    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
-    nextLane_tmp.curvature = tmp * 0.000001 + -0.031999;
-    //ServiceCarStatus.nextLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
-    //ServiceCarStatus.nextLaneA.curvature = tmp * 0.000001 + -0.031999;
-
-    tmp = 0;
-
-    //curvature derivative
-    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
-    nextLane_tmp.curvature_derivative = tmp * 0.000000 + -0.000122;
-    //ServiceCarStatus.nextLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
-    //ServiceCarStatus.nextLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
-
-    tmp = 0;
-
-    //width left marking
-    tmp = receivedata.Data[7];
-    nextLane_tmp.lane_mark_width = tmp * 0.01;
-    //ServiceCarStatus.nextLaneA.lane_mark_width = tmp * 0.01;
-
-    return nextLane_tmp;
-}
-
-/****************************************************  mobileye LKA right lane b ****************************************************************************
- *  message ID:0x76d
- *  member:                  type           physical unit       range               note
- *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
- *  view_range               double                m         [0, 127.996]        physical view range of lane mark
- *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
- *************************************************************************************************************************************************************/
-next_lane_b iv::control::CanUtil::impl_0x76d(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-
-    next_lane_b nextLane_tmp;
-
-    //heading angle
-    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
-    nextLane_tmp.heading_angle = tmp * 0.000977 + -31.999023;
-    //ServiceCarStatus.nextLaneB.heading_angle = (tmp - 0x7fff) / 1024;
-    //ServiceCarStatus.nextLaneB.heading_angle = tmp * 0.000977 + -31.999023;
-
-    tmp = 0;
-
-    //view range
-    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
-    nextLane_tmp.view_range = tmp * 0.003906;
-    //ServiceCarStatus.nextLaneB.view_range = tmp / 256.0;
-    //ServiceCarStatus.nextLaneB.view_range = tmp * 0.003906;
-
-    tmp = 0;
-
-    //view range availability
-    nextLane_tmp.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
-    //ServiceCarStatus.nextLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
-
-    return nextLane_tmp;
-}
-
-/****************************************************  mobileye reference points ******************************************************************************
- *  message ID:0x76a
- *  member:                  type           physical unit       range               note
- *  ref_point_1_position     double               m      [-127.996, 127.996]     physical distance between camera and reference point 1 on lateral axis.
- *                                                                               The reference point defines the lateral location of the lane center at ref-Point distance.
- *                                                                               ref Point 1 is this measurement at 1 second headway.
- *  ref_point_1_dist         double               m       [0, 127.99609376]      physical distance between reference point and camera
- *  ref_point_1_validity     enum                 /              0,1             0 invalid, 1 valid
- *  ref_point_2_position     double               m      [-127.996, 127.996]     empty! physical distance between camera and reference point 2 on lateral axis.
- *                                                                               The reference point defines the lateral location of the lane center at ref-Point distance.
- *                                                                               ref Point 2 is this measurement at 1 second headway.
- *  ref_point_2_dist         double               m       [0, 127.99609376]      empty! physical distance between reference point and camera
- *  ref_point_2_validity     enum                 /              0,1             empty! 0 invalid, 1 valid
- *************************************************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x76a(VCI_CAN_OBJ receivedata)
-{
-    int32_t tmp = 0;
-
-    //ref_point_1_position
-    //tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
-    ServiceCarStatus.refPoints.ref_point_1_position = (tmp - 0x7fff) / 256.0;
-    ServiceCarStatus.refPoints.ref_point_1_position = tmp * 0.003906 + -127.996094;
-
-    tmp = 0;
-
-    //ref_point_1_dist
-    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
-    //ServiceCarStatus.refPoints.ref_point_1_dist = tmp / 256.0;
-    ServiceCarStatus.refPoints.ref_point_1_dist = tmp * 0.003906;
-
-    tmp = 0;
-
-    //is ref point 1 validity
-    ServiceCarStatus.refPoints.is_ref_point_1_validity = (receivedata.Data[3] >> 7) & 0x1;
-
-    //ref_point_2_position
-    tmp = (receivedata.Data[5] << 8) | receivedata.Data[4];
-    //ServiceCarStatus.refPoints.ref_point_2_position = (tmp - 0x7fff) / 256.0;
-    ServiceCarStatus.refPoints.ref_point_2_position = tmp * 0.003906 + -127.996094;
-
-    tmp = 0;
-
-    //ref_point_2_dist
-    tmp = ((receivedata.Data[7] & 0x7f) << 8) | receivedata.Data[6];
-    //ServiceCarStatus.refPoints.ref_point_2_dist = tmp / 256.0;
-    ServiceCarStatus.refPoints.ref_point_2_dist = tmp * 0.003906;
-
-    tmp = 0;
-
-    //is ref point 2 validity
-    ServiceCarStatus.refPoints.is_ref_point_2_validity = (receivedata.Data[7] >> 7) & 0x1;
-}
-
-/**********************************************  mobileye number of next lane markers reported **************************************************************
- *  message ID:0x76b
- *  member:                             type           physical unit       range               note
- *  num_of_next_lane_mark_reported      enum                 /              1,2    indicates how many extra lane markers are also reported, on top of left
- *                                                                                 and right lane. Currently two lane marks are always reported one for left and one for
- *                                                                                 right, and they are valid only if the lane type is Solid.
- *************************************************************************************************************************************************************/
-void iv::control::CanUtil::impl_0x76b(VCI_CAN_OBJ receivedata)
-{
-    //num_of_next_lane_mark_reported
-    ServiceCarStatus.numOfNextLaneMarkReported.num_of_next_lane_mark_reported = receivedata.Data[0];
-}
-
-void iv::control::CanUtil::receive_radar() {
-    int32_t range, rate, angle;
-	while (m_connect)
-	{
-		VCI_CAN_OBJ receivedata[2500];
-		int res = VCI_Receive(m_devtype, m_devind, m_cannum_radar, receivedata, 2500, 10);
-		int radar_ID_index = -1;
-		if (res <= 0)	//未能从can卡中读到数
-		{
-#ifdef linux
-            usleep(1000);
-#endif
-#ifdef WIN32
-            Sleep(1);
-#endif
-            continue;
-		}
-        for (int i = 0; i < res; i++) {
-			//对应毫米波雷达数据解析
-			radar_ID_index = receivedata[i].ID - 0x500;
-			if (radar_ID_index >= 0x00 && radar_ID_index <= 0x3f) {
-                ServiceCarStatus.mRadarS = 10;
-				angle = ((receivedata[i].Data[1] & 0x1F) << 5) + ((receivedata[i].Data[2] & 0xF8) / 8);
-				range = ((receivedata[i].Data[2] & 0x07) << 8) + receivedata[i].Data[3];
-				rate = ((receivedata[i].Data[6] & 0x3F) << 8) | receivedata[i].Data[7];
-				if (angle & 0x200) {
-					angle = angle | 0xFFFFFC00;
-				}
-				if (rate & 0x2000) {
-					rate = rate | 0xFFFFC000;
-				}
-				//If angle and range both are 0, it is an invalid data.
-				if (angle != 0 || range != 0) {
-                    obs_find_flag = true;
-                    ServiceCarStatus.obs_radar[radar_ID_index].valid = true;
-                    ServiceCarStatus.obs_radar[radar_ID_index].nomal_x = range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI);
-                    ServiceCarStatus.obs_radar[radar_ID_index].nomal_y = range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI);
-                    ServiceCarStatus.obs_radar[radar_ID_index].speed_relative = rate * 1.0 / 100.0;
-                    ServiceCarStatus.obs_radar[radar_ID_index].speed_x = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * sin(angle / 1800.0 * M_PI);
-                    ServiceCarStatus.obs_radar[radar_ID_index].speed_y = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * cos(angle / 1800.0 * M_PI);
-				}
-				else {
-					ServiceCarStatus.obs_radar[radar_ID_index].valid = false;
-				}
-			}
-        }
-    }
-}
-
-void iv::control::CanUtil::obs_print(std::vector<obstacle_data_a> obstacleStatusA, std::vector<obstacle_data_b> obstacleStatusB, std::vector<obstacle_data_c> obstacleStatusC)
-{
-    int vec_size = obstacleStatusA.size();
-    if(vec_size == 0)
-    {
-        return;
-    }
-    std::cout<< "*****************************************************************************************"<<std::endl;
-    for(int i = 0; i < vec_size; i++)
-    {
-
-        std::cout<< "obstacle ID: " << obstacleStatusA[i].obstacle_ID << std::endl;
-        std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_x << std::endl;
-        std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_y << std::endl;
-        std::cout<< "obstacle_rel_vel_x: " << obstacleStatusA[i].obstacle_rel_vel_x << std::endl;
-        std::cout<< "obstacle_type: " << obstacleStatusA[i].obstacle_type << std::endl;
-        std::cout<< "obstacle_status: " << obstacleStatusA[i].obstacle_status << std::endl;
-        std::cout<< "obstacle_brake_light: " << obstacleStatusA[i].is_obstacle_brake_lights << std::endl;
-        std::cout<< "obstacle_valid: " << obstacleStatusA[i].obstacle_valid << std::endl;
-//        //std::cout<< "obstacle_length: " << obstacleStatusB[i].obstacle_length << std::endl;
-//        std::cout<< "obstacle_width: " << obstacleStatusB[i].obstacle_width << std::endl;
-//        std::cout<< "obstacle_age: " << obstacleStatusB[i].obstacle_age << std::endl;
-//        std::cout<< "obstacle_cipv: " << obstacleStatusB[i].is_cipv_flag << std::endl;
-//        std::cout<< "obstacle_radar_pos_x: " << obstacleStatusB[i].radar_pos_x << std::endl;
-//        std::cout<< "obstacle_radar_vel_x: " << obstacleStatusB[i].radar_vel_x << std::endl;
-//        std::cout<< "obstacle_angle_rate: " << obstacleStatusC[i].obstacle_angle_rate << std::endl;
-//        std::cout<< "obstacle_angle" << obstacleStatusC[i].obstacle_angle << std::endl;
-//        std::cout<< "object_accel_x" << obstacleStatusC[i].object_accel_x << std::endl;
-//        std::cout<< "obstacle_scale_change: " << obstacleStatusC[i].obstacle_scale_change << std::endl;
-//        std::cout<< "is_obstacle_replaced: " << obstacleStatusC[i].is_obstacle_replaced<< std::endl;
-    }
-    std::cout<< "*****************************************************************************************"<<std::endl;
-}
-
-#ifdef USE_MOBILEYE
-void iv::control::CanUtil::receive_mobileye(){
-    while(m_connect)
-    {
-        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_a) * 15);
-        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_b) * 15);
-        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_c) * 15);
-        int obs_a_count = 0, obs_b_count = 0, obs_c_count = 0;
-        int left_lane_a_count = 0, right_lane_a_count = 0, left_lane_b_count = 0, right_lane_b_count = 0;
-        obstacle_data_a a_tmp;
-        obstacle_data_b b_tmp;
-        obstacle_data_c c_tmp;
-        next_lane_a left_tmp_a, right_tmp_a;
-        next_lane_b left_tmp_b, right_tmp_b;
-        VCI_CAN_OBJ receivedata[2500];
-        int res = VCI_Receive(m_devtype, m_devindmob, m_cannum, receivedata, 2500, 10);
-        if (res <= 0)
-        {
-#ifdef linux
-            usleep(1000);
-#endif
-#ifdef WIN32
-            Sleep(1);
-#endif
-            continue;
-        }
-        for (int i = 0; i < res; i++)
-        {
-            switch(receivedata[i].ID)
-            {
-                case 0x700:
-                    impl_0x700(receivedata[i]);
-                    break;
-                case 0x737:
-                    impl_0x737(receivedata[i]);
-                    break;
-                case 0x738:
-                    impl_0x738(receivedata[i]);
-                    break;
-                case 0x739: case 0x73c: case 0x73f: case 0x742: case 0x745: case 0x748: case 0x74b: case 0x74e: case 0x751: case 0x754: case 0x757: case 0x75a: case 0x75d: case 0x760: case 0x763:
-                    a_tmp = impl_0x739(receivedata[i]);
-                    memcpy(&(ServiceCarStatus.obstacleStatusA[obs_a_count]), &a_tmp, sizeof(obstacle_data_a));
-                    obs_a_count++;
-                    break;
-                case 0x73a: case 0x73d: case 0x740: case 0x743: case 0x746: case 0x749: case 0x74c: case 0x74f: case 0x752: case 0x755: case 0x758: case 0x75b: case 0x75e: case 0x761: case 0x764:
-                    b_tmp = impl_0x73a(receivedata[i]);
-                    memcpy(&(ServiceCarStatus.obstacleStatusB[obs_b_count]), &b_tmp, sizeof(obstacle_data_b));
-                    obs_b_count++;
-                    break;
-                case 0x73b: case 0x73e: case 0x741: case 0x744: case 0x747: case 0x74a: case 0x74d: case 0x750: case 0x753: case 0x756: case 0x759: case 0x75c: case 0x75f: case 0x762: case 0x765:
-                    c_tmp = impl_0x73b(receivedata[i]);
-                    memcpy(&(ServiceCarStatus.obstacleStatusC[obs_a_count]), &c_tmp, sizeof(obstacle_data_c));
-                    obs_c_count++;
-                    break;
-                case 0x669:
-                    impl_0x669(receivedata[i]);
-                    break;
-                case 0x766:
-                    impl_0x766(receivedata[i]);
-                    break;
-                case 0x767:
-                    impl_0x767(receivedata[i]);
-                    break;
-                case 0x768:
-                    impl_0x768(receivedata[i]);
-                    break;
-                case 0x769:
-                    impl_0x769(receivedata[i]);
-                    break;
-                case 0x76c: case 0x76e: case 0x771: case 0x773: case 0x775: case 0x777: case 0x779: case 0x77b:
-                    if (receivedata[i].ID == 0x76c || receivedata[i].ID == 0x771 || receivedata[i].ID == 0x775 || receivedata[i].ID == 0x779)
-                    {
-                        left_tmp_a = impl_0x76c(receivedata[i]);
-                        memcpy(&(ServiceCarStatus.nextLaneA_left[left_lane_a_count]), &left_tmp_a, sizeof(next_lane_a));
-                        left_lane_a_count++;
-                    }
-                    else
-                    {
-                        right_tmp_a = impl_0x76c(receivedata[i]);
-                        memcpy(&(ServiceCarStatus.nextLaneA_right[right_lane_a_count]), &right_tmp_a, sizeof(next_lane_a));
-                        right_lane_a_count++;
-                    }
-                    //impl_0x76c(receivedata[i]);
-                    break;
-                case 0x76d: case 0x76f: case 0x772: case 0x774: case 0x776: case 0x778: case 0x77a: case 0x77c:
-                    if (receivedata[i].ID == 0x76d || receivedata[i].ID == 0x772 || receivedata[i].ID == 0x776 || receivedata[i].ID == 0x77a)
-                    {
-                        left_tmp_b = impl_0x76d(receivedata[i]);
-                        memcpy(&(ServiceCarStatus.nextLaneB_left[left_lane_b_count]), &left_tmp_b, sizeof(next_lane_b));
-                        left_lane_b_count++;
-                    }
-                    else
-                    {
-                        right_tmp_b = impl_0x76d(receivedata[i]);
-                        memcpy(&(ServiceCarStatus.nextLaneB_right[right_lane_b_count]), &right_tmp_b, sizeof(next_lane_b));
-                        right_lane_b_count++;
-                    }
-                    break;
-                case 0x76a:
-                    impl_0x76a(receivedata[i]);
-                    break;
-                case 0x76b:
-                    impl_0x76b(receivedata[i]);
-                    break;
-                default:
-                    break;
-            }
-        }
-       // obs_print(ServiceCarStatus.obstacleStatusA, ServiceCarStatus.obstacleStatusB, ServiceCarStatus.obstacleStatusC);
-    }
-}
-#endif
-
-//void iv::control::CanUtil::send() {
-	
-//    VCI_CAN_OBJ senddata;
-
-//	QTime time;
-//    int nTimeLastSend1;
-//	time.start();
-//    nTimeLastSend1 = time.elapsed();
-//    int nSend1Int = 10;  //10ms
-
-//	senddata.SendType = m_sendtype;
-//	senddata.ExternFlag = m_frameflag;
-//    senddata.RemoteFlag = m_frameformat;
-//    ServiceControlStatus.control_cmd_.Head = 0xEB;
-
-//	while (true) {
-//        usleep(1000);
-//		if ((time.elapsed() - nTimeLastSend1) >= nSend1Int)
-//        {
-//			nTimeLastSend1 = time.elapsed();
-//			senddata.ID = ServiceControlStatus.id;
-//            senddata.DataLen = 8;
-//            ServiceControlStatus.control_cmd_.CommunicationCount++;
-//            ServiceControlStatus.control_cmd_.CheckSum = (ServiceControlStatus.control_cmd_.Acceleration_frac + ServiceControlStatus.control_cmd_.Acceleration_int + ServiceControlStatus.control_cmd_.CommunicationCount + ServiceControlStatus.control_cmd_.Control + ServiceControlStatus.control_cmd_.Wheel_Angel[0] + ServiceControlStatus.control_cmd_.Wheel_Angel[1])%256;
-//            memcpy(senddata.Data, &ServiceControlStatus.control_cmd_.Head, 8);
-//            VCI_Transmit(m_devtype, m_devind, m_cannum, &senddata, 1);
-//            ServiceControlStatus.control_cmd_.Control &= 0xCB;
-//        }
-//	}
-//}
-
-
-void iv::control::CanUtil::startsend(bool start_or_stop)
-{
-    if(start_or_stop == true && send_connect == true && is_repeat == false)
-    {
-        is_repeat = true;
-        thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this)));			//启动发送线程
-    }
-    else if(start_or_stop == false && send_connect == true && is_repeat == true)
-    {
-        thread_send->interrupt();
-        thread_send->join();
-        is_repeat = false;
-    }
-}
-
-bool iv::control::CanUtil::did_radar_ok()
-{
-    bool temp = obs_find_flag;
-    obs_find_flag = false;
-    return temp;
-}
+#include <control/can.h>
+#include <control/control_status.h>
+#include <common/constants.h>
+#include <common/logout.h>
+#include <perception/sensor_radar.h>
+
+#include <QTime>
+
+#define VCI_USBCAN1		3
+#define VCI_USBCAN2		4
+#define VCI_USBCAN2A		4
+
+#define VCI_USBCAN_E_U 		20
+#define VCI_USBCAN_2E_U 	21
+
+#define VCI_ACCCODE_DEFAULT		0x00000000
+#define VCI_ACCMASK_DEFAULT		0xFFFFFFFF
+#define VCI_TIMER0_DEFAULT		0x00
+#define VCI_TIMER1_DEFAULT		0x1C
+#define VCI_FILTER_DEFAULT		1
+#define VCI_MODE_DEFAULT		0
+
+//函数调用返回状态值
+#define	STATUS_OK					1
+#define STATUS_ERR					0
+
+#define LOG_ENABLE
+#ifndef M_PI
+#define M_PI (3.1415926535897932384626433832795)
+#endif
+
+iv::control::CanUtil::CanUtil() {
+	obs_radar1 = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+	obs_radar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+	obs_radar_ui = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+#ifdef WIN32
+    m_hDLL = LoadLibrary(TEXT("ControlCAN.dll"));
+
+        VCI_OpenDevice = (LPVCI_OpenDevice)GetProcAddress(m_hDLL, "VCI_OpenDevice");//取得函数地址
+        VCI_CloseDevice = (LPVCI_CloseDevice)GetProcAddress(m_hDLL, "VCI_CloseDevice");
+        VCI_InitCAN = (LPVCI_InitCan)GetProcAddress(m_hDLL, "VCI_InitCAN");
+        VCI_ReadBoardInfo = (LPVCI_ReadBoardInfo)GetProcAddress(m_hDLL, "VCI_ReadBoardInfo");
+        VCI_ReadErrInfo = (LPVCI_ReadErrInfo)GetProcAddress(m_hDLL, "VCI_ReadErrInfo");
+        VCI_ReadCanStatus = (LPVCI_ReadCanStatus)GetProcAddress(m_hDLL, "VCI_ReadCANStatus");
+        VCI_GetReference = (LPVCI_GetReference)GetProcAddress(m_hDLL, "VCI_GetReference");
+        VCI_SetReference = (LPVCI_SetReference)GetProcAddress(m_hDLL, "VCI_SetReference");
+        VCI_GetReceiveNum = (LPVCI_GetReceiveNum)GetProcAddress(m_hDLL, "VCI_GetReceiveNum");
+        VCI_ClearBuffer = (LPVCI_ClearBuffer)GetProcAddress(m_hDLL, "VCI_ClearBuffer");
+        VCI_StartCAN = (LPVCI_StartCAN)GetProcAddress(m_hDLL, "VCI_StartCAN");
+        VCI_ResetCAN = (LPVCI_ResetCAN)GetProcAddress(m_hDLL, "VCI_ResetCAN");
+        VCI_Transmit = (LPVCI_Transmit)GetProcAddress(m_hDLL, "VCI_Transmit");
+        VCI_Receive = (LPVCI_Receive)GetProcAddress(m_hDLL, "VCI_Receive");
+        VCI_GetReference2 = (LPVCI_GetReference2)GetProcAddress(m_hDLL, "VCI_GetReference2");
+        VCI_SetReference2 = (LPVCI_SetReference2)GetProcAddress(m_hDLL, "VCI_SetReference2");
+        VCI_ResumeConfig = (LPVCI_ResumeConfig)GetProcAddress(m_hDLL, "VCI_ResumeConfig");
+        VCI_ConnectDevice = (LPVCI_ConnectDevice)GetProcAddress(m_hDLL, "VCI_ConnectDevice");
+        VCI_UsbDeviceReset = (LPVCI_UsbDeviceReset)GetProcAddress(m_hDLL, "VCI_UsbDeviceReset");
+#endif
+}
+iv::control::CanUtil::~CanUtil() {
+
+}
+
+void iv::control::CanUtil::openCanCard() {
+	VCI_INIT_CONFIG initconfig;
+	initconfig.AccCode = VCI_ACCCODE_DEFAULT;
+	initconfig.AccMask = VCI_ACCMASK_DEFAULT;
+	initconfig.Timing0 = VCI_TIMER0_DEFAULT;
+	initconfig.Timing1 = VCI_TIMER1_DEFAULT;
+	initconfig.Filter = VCI_FILTER_DEFAULT;
+	initconfig.Mode = VCI_MODE_DEFAULT;
+
+	int status = VCI_OpenDevice(m_devtype, m_devind, 0);		//打开设备
+    m_connect = -1;
+	if (status == 1)
+	{
+		int init = VCI_InitCAN(m_devtype, m_devind, m_cannum, &initconfig);	//初始化设备
+		if (init == 1)
+		{
+            if (VCI_StartCAN(m_devtype, m_devind, m_cannum) == 1) {
+                send_connect = true;
+                m_connect = 1;
+                thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this)));
+                thread_receive =  boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive, this)));
+            }
+            else {
+                throw 0;
+			}
+		}
+        else
+        {
+            throw 1;
+        }
+		//	打开2通道，供毫米波雷达读取数据用
+		init = VCI_InitCAN(m_devtype, m_devind, m_cannum_radar, &initconfig);	//初始化设备
+		if (init == 1)
+		{
+			if (VCI_StartCAN(m_devtype, m_devind, m_cannum_radar) == 1) {
+                m_connect = 1;
+                thread_radar_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_radar, this)));	//启动毫米波雷达接收线程5
+
+            }
+			else {
+                throw 2;
+			}
+			
+		}
+		else
+		{
+            throw 3;
+		}
+	}
+	else if(status == 0) {
+        throw 4;
+	}
+	else if (status == -1) {
+        throw 5;
+	}
+
+#ifdef USE_MOBILEYE
+    status = VCI_OpenDevice(m_devtype, m_devindmob, 0);		//打开设备
+    if (status == 1)
+    {
+        int init = VCI_InitCAN(m_devtype, m_devindmob, m_cannum, &initconfig);	//初始化设备
+        if (init == 1)
+        {
+            if (VCI_StartCAN(m_devtype, m_devindmob, m_cannum) == 1) {
+                m_connect = 1;
+                thread_mobileye_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_mobileye, this)));
+             }
+            else {
+                throw 0;
+            }
+        }
+        else
+        {
+            throw 1;
+        }
+    }
+    else if(status == 0) {
+        throw 4;
+    }
+    else if (status == -1) {
+        throw 5;
+    }
+
+#endif
+}
+
+void iv::control::CanUtil::closeCanCard() {
+    if(m_connect != 0)
+    {
+        thread_receive->interrupt();
+        thread_receive->join();
+        thread_send->interrupt();
+        thread_send->join();
+        thread_receive->interrupt();
+        thread_receive->join();
+        unsigned int rec = VCI_CloseDevice(m_devtype, m_devind);
+#ifdef USE_MOBILEYE
+        rec = VCI_CloseDevice(m_devtype, m_devindmob);
+#endif
+         std::cout << "close can:" << rec << std::endl;
+    }
+}
+
+void iv::control::CanUtil::receive() {
+    while (m_connect)
+    {
+        VCI_CAN_OBJ receivedata[2500];
+        int res = VCI_Receive(m_devtype, m_devind, m_cannum, receivedata, 2500, 10);
+        if (res > 0)	//未能从can卡中读到数
+        {
+   //         std::cout<<"reee"<<std::endl;
+            int i;
+            for(i=0;i<res;i++)
+            {
+                if(receivedata[i].ID == 0x282)
+                {
+                    int i;
+                    unsigned int xvalue[8];
+                    float fvalue[8];
+                    unsigned int x;
+                    for(i=0;i<8;i++)
+                    {
+                        memcpy(&x,&receivedata[i].Data[0],4);
+                        x = (x<<((7-i)*4))>>28;
+                        xvalue[i] = x;
+                        fvalue[i] = x;
+                        if(x == 0)fvalue[i] = -1;
+                        else fvalue[i] = (fvalue[i] - 1.0)*0.2;
+                    }
+                    ServiceCarStatus.multrasonic_obs.mfront_leftmid = fvalue[0];
+                    ServiceCarStatus.multrasonic_obs.mfront_left = fvalue[1];
+                    ServiceCarStatus.multrasonic_obs.mfront_right = fvalue[2];
+                    ServiceCarStatus.multrasonic_obs.mfront_rightmid = fvalue[3];
+                    ServiceCarStatus.multrasonic_obs.mrear_leftmid = fvalue[4];
+                    ServiceCarStatus.multrasonic_obs.mrear_left = fvalue[5];
+                    ServiceCarStatus.multrasonic_obs.mrear_right = fvalue[6];
+                    ServiceCarStatus.multrasonic_obs.mrear_rightmid = fvalue[7];
+
+//                    std::cout<<"rec oooo value is back "<<(int)(receivedata[i].Data[3])<<" head "<<(int)receivedata[i].Data[1]<<std::endl;
+                }
+            }
+        }
+        else
+        {
+            usleep(1000);
+        }
+    }
+}
+
+void iv::control::CanUtil::initial()
+{
+    ServiceControlStatus.command.byte[0] = 0xEB;
+
+    ServiceControlStatus.command.byte[1] = 0;
+
+    ServiceControlStatus.command.byte[2] = 0;
+
+    ServiceControlStatus.command.byte[3] = 0;
+
+    ServiceControlStatus.command.byte[4] = 0;
+
+    ServiceControlStatus.command.byte[5] = 0;
+
+    ServiceControlStatus.command.byte[6] = 0;
+
+    ServiceControlStatus.command.byte[7] = 0;
+}
+
+void iv::control::CanUtil::pack_command()
+{
+    ServiceControlStatus.command.bit.heartbreak++;
+
+    if (ServiceControlStatus.command.bit.heartbreak >= 256)
+        ServiceControlStatus.command.bit.heartbreak = 0;
+
+    ServiceControlStatus.command.byte[7] = (ServiceControlStatus.command.byte[1] + ServiceControlStatus.command.byte[2]
+        + ServiceControlStatus.command.byte[3] + ServiceControlStatus.command.byte[4]
+        + ServiceControlStatus.command.byte[5] + ServiceControlStatus.command.byte[6]) % 256;
+}
+
+void iv::control::CanUtil::send()
+{
+    VCI_CAN_OBJ Command_data;
+
+        QTime time1;
+
+        int LastTimeSend1;
+
+        time1.start();
+
+        LastTimeSend1 = time1.elapsed();
+
+        int Send1Int = 10;  //100ms
+
+        Command_data.SendType = m_sendtype;
+        Command_data.ExternFlag = m_frameflag;
+        Command_data.RemoteFlag = m_frameformat;
+
+        Command_data.ID = ServiceControlStatus.command_ID;
+        Command_data.DataLen = 8;
+
+        initial();
+
+        while (true)
+        {
+            int print_time = time1.elapsed() - LastTimeSend1;
+            if (print_time >= Send1Int)
+            {
+                LastTimeSend1 = time1.elapsed();
+
+                pack_command();
+
+                memcpy(Command_data.Data, ServiceControlStatus.command.byte, 8);
+
+                VCI_Transmit(m_devtype, m_devind, m_cannum, &Command_data, 1);
+
+                /*for (int c = 0; c < 8; c++)
+                ODS("%x  ", Command_data.Data[c]);
+
+                ODS("\n");*/
+
+                //ODS("    %d  \n", print_time);
+            }
+        }
+}
+
+/******************************************************mobileye AWS display*****************************************************
+ *  message ID:0x700
+ *  member:                 type            physical unit       range                   note
+ *  dusk_time:              double                /              0,1
+ *  night_time              double                /              0,1
+ *******************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x700(VCI_CAN_OBJ receivedata)
+{
+    ServiceCarStatus.aws_display.dusk_time = (receivedata.Data[0] >> 3) & 0x1;
+    ServiceCarStatus.aws_display.night_time = (receivedata.Data[0] >> 4) & 0x1;
+    ServiceCarStatus.aws_display.lanes_on = receivedata.Data[4] & 0x1;
+    //std::cout << "lanes_on:" << ServiceCarStatus.aws_display.lanes_on << std::endl;
+}
+
+/******************************************************mobileye lane***********************************************************
+ *  message ID:0x737
+ *  member:                 type            physical unit       range                   note
+ *  curvature:              double             1/meter      [-0.12, 0.12]       "a" in the equation:y = ax^2+bx+c
+ *  heading:                double                /          [-1.0, 1.0]        "b" in the equation:y = ax^2+bx+c
+ *  construction area:      bool                  /               /                       /
+ *  pitch angle:            double             radians      [-0.05, 0.05]      pitch of the host vehicle(derived from lanes analysis)
+ *  yaw angle:              double             radians            /            yaw of the host vehicle(derived from lanes analysis)
+ *  right_LDW:              bit/bool              /              0,1           0 stands for unavailable, 1 for available
+ *  left_LDW:               bit/bool              /              0,1           0 stands for unavailable, 1 for available
+ *******************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x737(VCI_CAN_OBJ receivedata)
+{
+    //lane curvature
+    int32_t tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
+    tmp <<= 16;
+    tmp >>= 16;
+    ServiceCarStatus.Lane.curvature = tmp * 3.81 * 1e-6;
+    //std::cout<<"curvature:"<<ServiceCarStatus.Lane.curvature<<std::endl;
+
+    tmp = 0;
+
+    //lane heading
+    tmp = ((receivedata.Data[3] & 0xf) << 8) | receivedata.Data[2];
+    tmp <<= 20;
+    tmp >>= 20;
+    ServiceCarStatus.Lane.heading = tmp * 0.0005;
+
+    tmp = 0;
+
+    //construction area
+    ServiceCarStatus.Lane.is_ca = (receivedata.Data[3] >> 4) & 0x1;
+
+    //left_LDW
+    ServiceCarStatus.Lane.is_left_LDW_available = (receivedata.Data[3] >> 5) & 0x1;
+
+    //right_LDW
+    ServiceCarStatus.Lane.is_right_LDW_available = (receivedata.Data[3] >> 6) & 0x1;
+
+    //yaw
+    tmp = (receivedata.Data[5] >> 8) | receivedata.Data[4];
+    tmp <<= 16;
+    tmp >>= 16;
+    //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0;
+    ServiceCarStatus.Lane.yaw = tmp * 0.000977;
+    std::cout << "yaw:"<< ServiceCarStatus.Lane.yaw<< std::endl;
+
+    tmp = 0;
+
+    //pitch
+    tmp = (receivedata.Data[7] >> 8) | receivedata.Data[6];
+    tmp <<= 16;
+    tmp >>= 16;
+    //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0 / 512;
+    ServiceCarStatus.Lane.yaw = tmp * 0.000002;
+    std::cout << "pitch:" << ServiceCarStatus.Lane.pitch << std::endl;
+}
+
+/****************************************************  mobileye obstacle status  ****************************************************************************
+ *  message ID:0x738
+ *  member:                  type           physical unit       range               note
+ *  num_obstacles            int                   /           [0:255]                /
+ *  timestamp                int                   ms          [0:255]          only the lowest 8 bits of the timestamp is given
+ *  app_version:             int                   /           [0:255]          vesion info consists of X.Y.Z.W
+ *  active_version           int/2bits             /            [0:3]           index of the active section of app_version signal
+ *  left_close_rang_cut_in   bool                  /             0,1            0 false, 1 true
+ *  right_close_rang_cut_in  bool                  /             0,1            0 false, 1 true
+ *  go                       enum                  /           0,1...15         0 stop, 1 go, 2 undecided, 3 driver decisions is required, 4-14 unused, 15 not calculated
+ *  protocol version         uchar/8bit            /          0x00...0xff             /
+ *  is_close_car             bool                  /             0,1            0 no close, 1 close car exists
+ *  failsafe                 int/4bit              /            [0:7]           failsafe situation, 0000 no failsafe, 0001 low sun, 0010 blur image, 0100 1000 unused
+ *************************************************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x738(VCI_CAN_OBJ receivedata)
+{
+    //num_obstacles
+    ServiceCarStatus.obstacleStatus.num_obstacles = receivedata.Data[0];
+    //std::cout << ServiceCarStatus.obstacleStatus.num_obstacles<< std::endl;
+
+
+    //timestamp
+    ServiceCarStatus.obstacleStatus.timestamp = receivedata.Data[1];
+
+    //app version
+    ServiceCarStatus.obstacleStatus.app_version = receivedata.Data[2];
+
+    //active version
+    ServiceCarStatus.obstacleStatus.active_version = receivedata.Data[3] & 0x3;
+
+    //is left close rang cut in
+    ServiceCarStatus.obstacleStatus.is_left_close_rang_cut_in = (receivedata.Data[3] >> 2) & 0x1;
+
+    //is right close rang cut in
+    ServiceCarStatus.obstacleStatus.is_right_close_rang_cut_in = (receivedata.Data[3] >> 3) & 0x1;
+
+    //go
+    ServiceCarStatus.obstacleStatus.go = receivedata.Data[3] >> 4;
+
+    //protocol version
+    ServiceCarStatus.obstacleStatus.protocol_version = receivedata.Data[4];
+
+    //close car
+    ServiceCarStatus.obstacleStatus.is_close_car = receivedata.Data[5] & 0x1;
+
+    //failsafe
+    ServiceCarStatus.obstacleStatus.failsafe = (receivedata.Data[5] >> 1) & 0xf;
+}
+
+/****************************************************  mobileye obstacle data a  ****************************************************************************
+ *  message ID:0x739
+ *  member:                  type           physical unit       range               note
+ *  obstacle_ID              int                   /           [0:63]           new obstacles are given the last used free ID
+ *  obstacle_pos_x           double                m           [0,250]          longtitude position of the obstacle relative to the ref point
+ *  obstacle_pos_y           double                m       [-31.93,31.93]       lateral position of the obstacle, error relative to pos_x  below 10% or 2meters
+ *  blinker_info             int/enum              /            [0:4]           0 unavailable, 1 off, 2 left, 3 right, 4 both   indicated blinkers status
+ *  cut_in_and_out           int                   /            [0:4]           0 undefined, 1 in_host_lane, 2 out_host_lane, 3 cut_in, 4 cut_out
+ *                                                                              cut_in: target entering the host lane, cut_out: exiting  but not distinguish between sides
+ *  obstacle_rel_vel_x       double               m/s      [-127.93,127.93]     relative longtitude velocity of the obstacle
+ *  obstacle_status          int/enum              /            [0:6]           0 undefined, 1 standing(never moved, back lights on), 2 stopped(moveable), 3 moving
+ *                                                                              4 oncoming,  5 parked(never moved, back lights off), 6 unused
+ *  is_obstacle_brake_lights bool                  /             0,1            0 object's brake light off or not identified, 1 object's brake light on
+ *  obstacle_valid           int/enum              /            [1:2]           1 new valid(detected this frame), 2 older valid
+ *************************************************************************************************************************************************************/
+obstacle_data_a iv::control::CanUtil::impl_0x739(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+    obstacle_data_a obs_tmp;
+
+    //obstacle id
+    obs_tmp.obstacle_ID = receivedata.Data[0];
+    //ServiceCarStatus.obstacleStatusA.obstacle_ID = receivedata.Data[0];
+
+    //obstacle pos x
+    tmp = ((receivedata.Data[2] & 0xf) << 8) | receivedata.Data[1];
+    obs_tmp.obstacle_pos_x = tmp * 0.0625;
+    //ServiceCarStatus.obstacleStatusA.obstacle_pos_x = tmp * 0.0625;
+
+    tmp = 0;
+
+    //obstacle pos y
+    tmp = ((receivedata.Data[4] & 0x3) << 8) | receivedata.Data[3];
+    obs_tmp.obstacle_pos_y = tmp * 0.0625;
+    //ServiceCarStatus.obstacleStatusA.obstacle_pos_y = tmp * 0.0625;
+
+    tmp = 0;
+    //blinker info
+    obs_tmp.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
+    //ServiceCarStatus.obstacleStatusA.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
+
+    //cut in and out
+    obs_tmp.cut_in_and_out = receivedata.Data[4] >> 5;
+    //ServiceCarStatus.obstacleStatusA.cut_in_and_out = receivedata.Data[4] >> 5;
+
+    //obstacle rel vel x
+    tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
+    obs_tmp.obstacle_rel_vel_x = tmp * 0.0625;
+    //ServiceCarStatus.obstacleStatusA.obstacle_rel_vel_x = tmp * 0.0625;
+
+    tmp = 0;
+
+    //obstacle type
+    obs_tmp.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
+    //ServiceCarStatus.obstacleStatusA.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
+
+    //obstacle status
+    obs_tmp.obstacle_status = receivedata.Data[7] & 0x7;
+    //ServiceCarStatus.obstacleStatusA.obstacle_status = receivedata.Data[7] & 0x7;
+
+    //obstacle brake lights
+    obs_tmp.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
+    //ServiceCarStatus.obstacleStatusA.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
+
+    //obstacle valid
+    obs_tmp.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
+    //ServiceCarStatus.obstacleStatusA.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
+    return obs_tmp;
+}
+
+/****************************************************  mobileye obstacle data b  ****************************************************************************
+ *  message ID:0x73a
+ *  member:                  type           physical unit       range               note
+ *  obstacle_length          double                m           [0,31]           length of the obstacle(longitude axis)
+ *  obstacle_width           double                m           [0,12.5]         width of  the obstacle(lateral axis)
+ *  obstacle_age             int                   /           [0:255]          value starts at 1 when it is first detected, increaments in i each frame
+ *  obstacle_lane            int/enum              /           [0:3]            0 not assigned, 1 ego lane, 2 next lane(left or right) or next next lane,
+ *                                                                              3 invalid signal
+ *  is_cipv_flag             int/enum              /            [0:1]           0 not cipv, 1 cipv(closest in path vehicle)
+ *  radar_pos_x              double                m           [0,250]          longtitude postion of the primary radar target matched to the vision target, dist=relative to ref point
+ *                                                                              if no radar target is matched, value = 0xfff
+ *  radar_vel_x              double               m/s       [-127.93,127.93]    longitude velocity of the radar target matched to the vision targets, if no radar target is matched value=0xfff
+ *                                                                              4 oncoming,  5 parked(never moved, back lights off), 6 unused
+ *  radar_match_confidence   int                   /            [0:5]           0 no match, 1 multi match: radar doesn't describe well, 2-4 vision-radar match:with bounded error between
+ *                                                                              vision and radar measurements higher->smaller error, 5 high confidence match
+ *  matched_radar_id         int                   /           [0:127]          ID of Primary radar target matched to the vision target if applicable
+ *************************************************************************************************************************************************************/
+obstacle_data_b iv::control::CanUtil::impl_0x73a(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+    obstacle_data_b obs_tmp;
+
+    //obstacle length
+    tmp = receivedata.Data[0];
+    obs_tmp.obstacle_length = tmp * 0.5;
+    //ServiceCarStatus.obstacleStatusB.obstacle_length = tmp * 0.5;
+
+    tmp = 0;
+
+    //obstacle width
+    tmp = receivedata.Data[1];
+    obs_tmp.obstacle_width = tmp * 0.05;
+    //ServiceCarStatus.obstacleStatusB.obstacle_width = tmp * 0.05;
+
+    tmp = 0;
+
+    //obstacle age
+    obs_tmp.obstacle_age = receivedata.Data[2];
+    //ServiceCarStatus.obstacleStatusB.obstacle_age = receivedata.Data[2];
+
+    //obstacle lane
+    obs_tmp.obstacle_lane = receivedata.Data[3] & 0x3;
+    //ServiceCarStatus.obstacleStatusB.obstacle_lane = receivedata.Data[3] & 0x3;
+
+    //is cipv flag
+    obs_tmp.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
+    //ServiceCarStatus.obstacleStatusB.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
+
+    //radar pos x
+    tmp = (receivedata.Data[4] << 4) | (receivedata.Data[3] >> 4);
+    obs_tmp.radar_pos_x = tmp * 0.0625;
+    //ServiceCarStatus.obstacleStatusB.radar_pos_x = tmp * 0.0625;
+
+    tmp = 0;
+
+    //radar vel x
+    tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
+    tmp <<= 20;
+    tmp >>= 20;
+    obs_tmp.radar_vel_x = tmp * 0.0625;
+    //ServiceCarStatus.obstacleStatusB.radar_vel_x = tmp * 0.0625;
+
+    tmp = 0;
+
+    //radar match confidence
+    obs_tmp.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
+    //ServiceCarStatus.obstacleStatusB.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
+
+    //matched radar ID
+    obs_tmp.matched_radar_id = receivedata.Data[7] & 0x7f;
+    //ServiceCarStatus.obstacleStatusB.matched_radar_id = receivedata.Data[7] & 0x7f;
+
+    return obs_tmp;
+}
+
+/****************************************************  mobileye obstacle data c  ****************************************************************************
+ *  message ID:0x73b
+ *  member:                  type           physical unit       range               note
+ *  obstacle_angle_rate      double           degree/s     [-327.68, 327.68]    Angle rate of Center of Obstacle, negative->moved to left
+ *  obstcale_scale_change    double            pix/s       [-6.5532, 6.5532]    /
+ *  object_accel_x           double            m/s2         [-14.97, 14.97]     longtitude acceleration of the object
+ *  obstacle_replaced        bool                /               0,1            0 not replaced in this frame, 1 replace in this frame
+ *  obstacle_angle           double           degree        [-327.68,327.68]    Angle to Center of Obstacle in degrees, 0 indicates that the obstacle is in
+ *                                                                              exactly in front of us, a positive angle indicates that theobstacle is to the right
+ *************************************************************************************************************************************************************/
+obstacle_data_c iv::control::CanUtil::impl_0x73b(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+    obstacle_data_c obs_tmp;
+
+    //obstacle angle rate
+    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
+    tmp <<= 16;
+    tmp >>= 16;
+    obs_tmp.obstacle_angle_rate = tmp * 0.01;
+    //ServiceCarStatus.obstacleStatusC.obstacle_angle_rate = tmp * 0.01;
+
+    tmp = 0;
+
+    //obstacle scale change
+    tmp = (receivedata.Data[3] << 8) | receivedata.Data[2];
+    tmp <<= 16;
+    tmp >>= 16;
+    obs_tmp.obstacle_scale_change = tmp * 0.0002;
+    //ServiceCarStatus.obstacleStatusC.obstacle_scale_change = tmp * 0.0002;
+
+    tmp = 0;
+
+    //object accel x
+    tmp = ((receivedata.Data[5] & 0x3) << 8) | receivedata.Data[4];
+    tmp <<= 22;
+    tmp >>= 22;
+    obs_tmp.object_accel_x = tmp * 0.03;
+    //ServiceCarStatus.obstacleStatusC.object_accel_x = tmp * 0.03;
+
+    tmp = 0;
+
+    //obstacle replaced
+    obs_tmp.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
+    //ServiceCarStatus.obstacleStatusC.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
+
+    //obstacle angle
+    tmp = (receivedata.Data[7] << 8) | receivedata.Data[6];
+    tmp <<= 16;
+    tmp >>= 16;
+    obs_tmp.obstacle_angle = tmp * 0.01;
+    //ServiceCarStatus.obstacleStatusC.obstacle_angle = tmp * 0.01;
+
+    return obs_tmp;
+}
+
+/****************************************************  mobileye aftermarket lane  ****************************************************************************
+ *  message ID:0x669
+ *  member:                  type           physical unit       range               note
+ *  lane_conf_left           int/2bit              /            [0:3]            0 lowest, 3 highest
+ *  is_ldw_availablity_left  bool                  /             0,1             lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
+ *  lane_type_left           int/enum              /            [0:6]            0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
+ *  dist_to_lane_l           double                m           [-40:40]          "c" in the equation:y = ax^2+bx+c
+ *  lane_conf_right          int/2bit              /            [0:3]            0 lowest, 3 highest
+ *  is_ldw_availablity_right  bool                  /             0,1             lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
+ *  lane_type_right          int/enum              /            [0:6]            0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
+ *  dist_to_lane_r           double                m           [-40:40]          "c" in the equation:y = ax^2+bx+c
+ *************************************************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x669(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp;
+    //lane confidence left
+    tmp = receivedata.Data[0] & 0x3;
+    tmp <<= 30;
+    tmp >>= 30;
+    ServiceCarStatus.aftermarketLane.lane_conf_left = tmp;
+    //std::cout << "left confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_left << std::endl;
+
+    tmp = 0;
+
+    //is LDW availability left
+    ServiceCarStatus.aftermarketLane.is_ldw_availablility_left = (receivedata.Data[0] >> 2) & 0x1;
+
+    //lane type left
+    tmp = (receivedata.Data[0] >> 4) & 0xf;
+    tmp <<= 28;
+    tmp >>= 28;
+    ServiceCarStatus.aftermarketLane.lane_type_left = tmp;
+    std::cout << "lane_type_left: " << ServiceCarStatus.aftermarketLane.lane_type_left << std::endl;
+
+    tmp = 0;
+
+    //dist to left lane
+
+    tmp = (receivedata.Data[2] << 4) | ((receivedata.Data[1] >> 4) & 0xf);
+    tmp <<= 20;
+    tmp >>= 20;
+    ServiceCarStatus.aftermarketLane.dist_to_lane_l = tmp * 0.02;
+    std::cout << "dist_to_lane_l:" << ServiceCarStatus.aftermarketLane.dist_to_lane_l << std::endl;
+
+    tmp = 0;
+
+    //lane confidence right
+    tmp = receivedata.Data[5] & 0x3;
+    tmp <<= 30;
+    tmp >>= 30;
+    ServiceCarStatus.aftermarketLane.lane_conf_right = tmp;
+    //std::cout << "right confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_right << std::endl;
+
+    tmp = 0;
+
+    //is LDW availability right
+    ServiceCarStatus.aftermarketLane.is_ldw_availablility_right = (receivedata.Data[5] >> 2) & 0x1;
+
+    //lane type right
+    tmp = (receivedata.Data[5] >> 4) & 0xf;
+    tmp <<= 28;
+    tmp >>= 28;
+    ServiceCarStatus.aftermarketLane.lane_type_right = tmp;
+    std::cout<<"lane_type_right: " << ServiceCarStatus.aftermarketLane.lane_type_right << std::endl;
+
+    tmp = 0;
+
+    //dist to right lane
+    tmp = (receivedata.Data[7] << 4) | ((receivedata.Data[6] >> 4) & 0xf);
+    tmp <<= 20;
+    tmp >>= 20;
+    ServiceCarStatus.aftermarketLane.dist_to_lane_r = tmp * 0.02;
+    std::cout << "dist_to_lane_r:" << ServiceCarStatus.aftermarketLane.dist_to_lane_r << std::endl;
+
+    tmp = 0;
+}
+
+/****************************************************  mobileye LKA left lane a ****************************************************************************
+ *  message ID:0x766
+ *  member:                  type           physical unit       range               note
+ *  lane_type                enum                  /            [0:6]            0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
+ *  quality                  enum                  /            [0:3]            0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
+ *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
+ *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
+ *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
+ *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
+ *  width_left_marking       double                m           [0, 2.5]          left lane marking width
+ *************************************************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x766(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+
+    //lane type
+    ServiceCarStatus.LKAleftLaneA.lane_type = receivedata.Data[0] & 0xf;
+
+    //quality
+    ServiceCarStatus.LKAleftLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
+
+    //model degree
+    ServiceCarStatus.LKAleftLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
+
+    //position
+    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
+    tmp <<= 16;
+    tmp >>= 16;
+    ServiceCarStatus.LKAleftLaneA.position = tmp / 256.0;
+    //ServiceCarStatus.LKAleftLaneA.position = tmp * 0.003906;
+
+    tmp = 0;
+
+    //curvature
+    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
+    ServiceCarStatus.LKAleftLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
+    //ServiceCarStatus.LKAleftLaneA.curvature = tmp * 0.000001 + -0.031999;
+
+    tmp = 0;
+
+    //curvature derivative
+    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
+    ServiceCarStatus.LKAleftLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
+    //ServiceCarStatus.LKAleftLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
+
+    tmp = 0;
+
+    //width left marking
+    tmp = receivedata.Data[7];
+    ServiceCarStatus.LKAleftLaneA.width_left_marking = tmp * 0.01;
+
+}
+
+/****************************************************  mobileye LKA left lane b ****************************************************************************
+ *  message ID:0x767
+ *  member:                  type           physical unit       range               note
+ *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
+ *  view_range               double                m         [0, 127.996]        physical view range of lane mark
+ *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
+ *************************************************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x767(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+
+    //heading angle
+    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
+    ServiceCarStatus.LKAleftLaneB.heading_angle = (tmp - 0x7fff) / 1024;
+    //ServiceCarStatus.LKAleftLaneB.heading_angle = tmp * 0.000977 + -31.999023;
+
+    tmp = 0;
+
+    //view range
+    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
+    ServiceCarStatus.LKAleftLaneB.view_range = tmp / 256.0;
+    //ServiceCarStatus.LKAleftLaneB.view_range = tmp * 0.003906;
+
+    tmp = 0;
+
+    //view range availability
+    ServiceCarStatus.LKAleftLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
+
+}
+
+/****************************************************  mobileye LKA right lane a ****************************************************************************
+ *  message ID:0x768
+ *  member:                  type           physical unit       range               note
+ *  lane_type                enum                  /            [0:6]            0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
+ *  quality                  enum                  /            [0:3]            0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
+ *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
+ *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
+ *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
+ *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
+ *  width_right_marking      double                m           [0, 2.5]          right lane marking width
+ *************************************************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x768(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+
+    //lane type
+    ServiceCarStatus.LKArightLaneA.lane_type = receivedata.Data[0] & 0xf;
+
+    //quality
+    ServiceCarStatus.LKArightLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
+
+    //model degree
+    ServiceCarStatus.LKArightLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
+
+    //position
+    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
+    tmp <<= 16;
+    tmp >>= 16;
+    ServiceCarStatus.LKArightLaneA.position = tmp / 256.0;
+    //ServiceCarStatus.LKArightLaneA.position = tmp * 0.003906;
+
+    tmp = 0;
+
+    //curvature
+    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
+    ServiceCarStatus.LKArightLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
+    //ServiceCarStatus.LKArightLaneA.curvature = tmp * 0.000001 + -0.031999;
+
+    tmp = 0;
+
+    //curvature derivative
+    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
+    ServiceCarStatus.LKArightLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
+    //ServiceCarStatus.LKArightLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
+
+    tmp = 0;
+
+    //width left marking
+    tmp = receivedata.Data[7];
+    ServiceCarStatus.LKArightLaneA.width_left_marking = tmp * 0.01;
+}
+
+/****************************************************  mobileye LKA right lane b ****************************************************************************
+ *  message ID:0x767
+ *  member:                  type           physical unit       range               note
+ *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
+ *  view_range               double                m         [0, 127.996]        physical view range of lane mark
+ *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
+ *************************************************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x769(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+
+    //heading angle
+    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
+    ServiceCarStatus.LKArightLaneB.heading_angle = (tmp - 0x7fff) / 1024;
+    //ServiceCarStatus.LKArightLaneB.heading_angle = tmp * 0.000977 + -31.999023;
+
+    tmp = 0;
+
+    //view range
+    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
+    ServiceCarStatus.LKArightLaneB.view_range = tmp / 256.0;
+    //ServiceCarStatus.LKArightLaneB.view_range = tmp * 0.003906;
+
+    tmp = 0;
+
+    //view range availability
+    ServiceCarStatus.LKArightLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
+
+}
+
+/****************************************************  mobileye next lane a **********************************************************************************
+ *  message ID:0x76c
+ *  member:                  type           physical unit       range               note
+ *  lane_type                enum                  /            [1:2]            1 solid, 2 undecided
+ *  quality                    /                   /              /              not yet implemented for next lanes
+ *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
+ *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
+ *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
+ *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
+ *  lane_mark_widt           double                m           [0, 2.5]          lane marking width
+ *************************************************************************************************************************************************************/
+next_lane_a iv::control::CanUtil::impl_0x76c(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+    next_lane_a nextLane_tmp;
+
+    //lane type
+    nextLane_tmp.lane_type = receivedata.Data[0] & 0xf;
+    //ServiceCarStatus.nextLaneA.lane_type = receivedata.Data[0] & 0xf;
+
+    //quality
+    nextLane_tmp.quality = (receivedata.Data[0] >> 4) & 0x3;
+    //ServiceCarStatus.nextLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
+
+    //model degree
+    nextLane_tmp.model_degree = (receivedata.Data[0] >> 6) & 0x3;
+    //ServiceCarStatus.nextLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
+
+    //position
+    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
+    tmp <<= 16;
+    tmp >>= 16;
+    nextLane_tmp.position = tmp * 0.003906;
+    //ServiceCarStatus.nextLaneA.position = tmp / 256.0;
+    //ServiceCarStatus.nextLaneA.position = tmp * 0.003906;
+
+    tmp = 0;
+
+    //curvature
+    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
+    nextLane_tmp.curvature = tmp * 0.000001 + -0.031999;
+    //ServiceCarStatus.nextLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
+    //ServiceCarStatus.nextLaneA.curvature = tmp * 0.000001 + -0.031999;
+
+    tmp = 0;
+
+    //curvature derivative
+    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
+    nextLane_tmp.curvature_derivative = tmp * 0.000000 + -0.000122;
+    //ServiceCarStatus.nextLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
+    //ServiceCarStatus.nextLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
+
+    tmp = 0;
+
+    //width left marking
+    tmp = receivedata.Data[7];
+    nextLane_tmp.lane_mark_width = tmp * 0.01;
+    //ServiceCarStatus.nextLaneA.lane_mark_width = tmp * 0.01;
+
+    return nextLane_tmp;
+}
+
+/****************************************************  mobileye LKA right lane b ****************************************************************************
+ *  message ID:0x76d
+ *  member:                  type           physical unit       range               note
+ *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
+ *  view_range               double                m         [0, 127.996]        physical view range of lane mark
+ *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
+ *************************************************************************************************************************************************************/
+next_lane_b iv::control::CanUtil::impl_0x76d(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+
+    next_lane_b nextLane_tmp;
+
+    //heading angle
+    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
+    nextLane_tmp.heading_angle = tmp * 0.000977 + -31.999023;
+    //ServiceCarStatus.nextLaneB.heading_angle = (tmp - 0x7fff) / 1024;
+    //ServiceCarStatus.nextLaneB.heading_angle = tmp * 0.000977 + -31.999023;
+
+    tmp = 0;
+
+    //view range
+    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
+    nextLane_tmp.view_range = tmp * 0.003906;
+    //ServiceCarStatus.nextLaneB.view_range = tmp / 256.0;
+    //ServiceCarStatus.nextLaneB.view_range = tmp * 0.003906;
+
+    tmp = 0;
+
+    //view range availability
+    nextLane_tmp.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
+    //ServiceCarStatus.nextLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
+
+    return nextLane_tmp;
+}
+
+/****************************************************  mobileye reference points ******************************************************************************
+ *  message ID:0x76a
+ *  member:                  type           physical unit       range               note
+ *  ref_point_1_position     double               m      [-127.996, 127.996]     physical distance between camera and reference point 1 on lateral axis.
+ *                                                                               The reference point defines the lateral location of the lane center at ref-Point distance.
+ *                                                                               ref Point 1 is this measurement at 1 second headway.
+ *  ref_point_1_dist         double               m       [0, 127.99609376]      physical distance between reference point and camera
+ *  ref_point_1_validity     enum                 /              0,1             0 invalid, 1 valid
+ *  ref_point_2_position     double               m      [-127.996, 127.996]     empty! physical distance between camera and reference point 2 on lateral axis.
+ *                                                                               The reference point defines the lateral location of the lane center at ref-Point distance.
+ *                                                                               ref Point 2 is this measurement at 1 second headway.
+ *  ref_point_2_dist         double               m       [0, 127.99609376]      empty! physical distance between reference point and camera
+ *  ref_point_2_validity     enum                 /              0,1             empty! 0 invalid, 1 valid
+ *************************************************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x76a(VCI_CAN_OBJ receivedata)
+{
+    int32_t tmp = 0;
+
+    //ref_point_1_position
+    //tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
+    ServiceCarStatus.refPoints.ref_point_1_position = (tmp - 0x7fff) / 256.0;
+    ServiceCarStatus.refPoints.ref_point_1_position = tmp * 0.003906 + -127.996094;
+
+    tmp = 0;
+
+    //ref_point_1_dist
+    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
+    //ServiceCarStatus.refPoints.ref_point_1_dist = tmp / 256.0;
+    ServiceCarStatus.refPoints.ref_point_1_dist = tmp * 0.003906;
+
+    tmp = 0;
+
+    //is ref point 1 validity
+    ServiceCarStatus.refPoints.is_ref_point_1_validity = (receivedata.Data[3] >> 7) & 0x1;
+
+    //ref_point_2_position
+    tmp = (receivedata.Data[5] << 8) | receivedata.Data[4];
+    //ServiceCarStatus.refPoints.ref_point_2_position = (tmp - 0x7fff) / 256.0;
+    ServiceCarStatus.refPoints.ref_point_2_position = tmp * 0.003906 + -127.996094;
+
+    tmp = 0;
+
+    //ref_point_2_dist
+    tmp = ((receivedata.Data[7] & 0x7f) << 8) | receivedata.Data[6];
+    //ServiceCarStatus.refPoints.ref_point_2_dist = tmp / 256.0;
+    ServiceCarStatus.refPoints.ref_point_2_dist = tmp * 0.003906;
+
+    tmp = 0;
+
+    //is ref point 2 validity
+    ServiceCarStatus.refPoints.is_ref_point_2_validity = (receivedata.Data[7] >> 7) & 0x1;
+}
+
+/**********************************************  mobileye number of next lane markers reported **************************************************************
+ *  message ID:0x76b
+ *  member:                             type           physical unit       range               note
+ *  num_of_next_lane_mark_reported      enum                 /              1,2    indicates how many extra lane markers are also reported, on top of left
+ *                                                                                 and right lane. Currently two lane marks are always reported one for left and one for
+ *                                                                                 right, and they are valid only if the lane type is Solid.
+ *************************************************************************************************************************************************************/
+void iv::control::CanUtil::impl_0x76b(VCI_CAN_OBJ receivedata)
+{
+    //num_of_next_lane_mark_reported
+    ServiceCarStatus.numOfNextLaneMarkReported.num_of_next_lane_mark_reported = receivedata.Data[0];
+}
+
+void iv::control::CanUtil::receive_radar() {
+    int32_t range, rate, angle;
+	while (m_connect)
+	{
+		VCI_CAN_OBJ receivedata[2500];
+		int res = VCI_Receive(m_devtype, m_devind, m_cannum_radar, receivedata, 2500, 10);
+		int radar_ID_index = -1;
+		if (res <= 0)	//未能从can卡中读到数
+		{
+#ifdef linux
+            usleep(1000);
+#endif
+#ifdef WIN32
+            Sleep(1);
+#endif
+            continue;
+		}
+        for (int i = 0; i < res; i++) {
+			//对应毫米波雷达数据解析
+			radar_ID_index = receivedata[i].ID - 0x500;
+			if (radar_ID_index >= 0x00 && radar_ID_index <= 0x3f) {
+                ServiceCarStatus.mRadarS = 10;
+				angle = ((receivedata[i].Data[1] & 0x1F) << 5) + ((receivedata[i].Data[2] & 0xF8) / 8);
+				range = ((receivedata[i].Data[2] & 0x07) << 8) + receivedata[i].Data[3];
+				rate = ((receivedata[i].Data[6] & 0x3F) << 8) | receivedata[i].Data[7];
+				if (angle & 0x200) {
+					angle = angle | 0xFFFFFC00;
+				}
+				if (rate & 0x2000) {
+					rate = rate | 0xFFFFC000;
+				}
+				//If angle and range both are 0, it is an invalid data.
+				if (angle != 0 || range != 0) {
+                    obs_find_flag = true;
+                    ServiceCarStatus.obs_radar[radar_ID_index].valid = true;
+                    ServiceCarStatus.obs_radar[radar_ID_index].nomal_x = range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI);
+                    ServiceCarStatus.obs_radar[radar_ID_index].nomal_y = range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI);
+                    ServiceCarStatus.obs_radar[radar_ID_index].speed_relative = rate * 1.0 / 100.0;
+                    ServiceCarStatus.obs_radar[radar_ID_index].speed_x = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * sin(angle / 1800.0 * M_PI);
+                    ServiceCarStatus.obs_radar[radar_ID_index].speed_y = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * cos(angle / 1800.0 * M_PI);
+				}
+				else {
+					ServiceCarStatus.obs_radar[radar_ID_index].valid = false;
+				}
+			}
+        }
+    }
+}
+
+void iv::control::CanUtil::obs_print(std::vector<obstacle_data_a> obstacleStatusA, std::vector<obstacle_data_b> obstacleStatusB, std::vector<obstacle_data_c> obstacleStatusC)
+{
+    int vec_size = obstacleStatusA.size();
+    if(vec_size == 0)
+    {
+        return;
+    }
+    std::cout<< "*****************************************************************************************"<<std::endl;
+    for(int i = 0; i < vec_size; i++)
+    {
+
+        std::cout<< "obstacle ID: " << obstacleStatusA[i].obstacle_ID << std::endl;
+        std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_x << std::endl;
+        std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_y << std::endl;
+        std::cout<< "obstacle_rel_vel_x: " << obstacleStatusA[i].obstacle_rel_vel_x << std::endl;
+        std::cout<< "obstacle_type: " << obstacleStatusA[i].obstacle_type << std::endl;
+        std::cout<< "obstacle_status: " << obstacleStatusA[i].obstacle_status << std::endl;
+        std::cout<< "obstacle_brake_light: " << obstacleStatusA[i].is_obstacle_brake_lights << std::endl;
+        std::cout<< "obstacle_valid: " << obstacleStatusA[i].obstacle_valid << std::endl;
+//        //std::cout<< "obstacle_length: " << obstacleStatusB[i].obstacle_length << std::endl;
+//        std::cout<< "obstacle_width: " << obstacleStatusB[i].obstacle_width << std::endl;
+//        std::cout<< "obstacle_age: " << obstacleStatusB[i].obstacle_age << std::endl;
+//        std::cout<< "obstacle_cipv: " << obstacleStatusB[i].is_cipv_flag << std::endl;
+//        std::cout<< "obstacle_radar_pos_x: " << obstacleStatusB[i].radar_pos_x << std::endl;
+//        std::cout<< "obstacle_radar_vel_x: " << obstacleStatusB[i].radar_vel_x << std::endl;
+//        std::cout<< "obstacle_angle_rate: " << obstacleStatusC[i].obstacle_angle_rate << std::endl;
+//        std::cout<< "obstacle_angle" << obstacleStatusC[i].obstacle_angle << std::endl;
+//        std::cout<< "object_accel_x" << obstacleStatusC[i].object_accel_x << std::endl;
+//        std::cout<< "obstacle_scale_change: " << obstacleStatusC[i].obstacle_scale_change << std::endl;
+//        std::cout<< "is_obstacle_replaced: " << obstacleStatusC[i].is_obstacle_replaced<< std::endl;
+    }
+    std::cout<< "*****************************************************************************************"<<std::endl;
+}
+
+#ifdef USE_MOBILEYE
+void iv::control::CanUtil::receive_mobileye(){
+    while(m_connect)
+    {
+        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_a) * 15);
+        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_b) * 15);
+        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_c) * 15);
+        int obs_a_count = 0, obs_b_count = 0, obs_c_count = 0;
+        int left_lane_a_count = 0, right_lane_a_count = 0, left_lane_b_count = 0, right_lane_b_count = 0;
+        obstacle_data_a a_tmp;
+        obstacle_data_b b_tmp;
+        obstacle_data_c c_tmp;
+        next_lane_a left_tmp_a, right_tmp_a;
+        next_lane_b left_tmp_b, right_tmp_b;
+        VCI_CAN_OBJ receivedata[2500];
+        int res = VCI_Receive(m_devtype, m_devindmob, m_cannum, receivedata, 2500, 10);
+        if (res <= 0)
+        {
+#ifdef linux
+            usleep(1000);
+#endif
+#ifdef WIN32
+            Sleep(1);
+#endif
+            continue;
+        }
+        for (int i = 0; i < res; i++)
+        {
+            switch(receivedata[i].ID)
+            {
+                case 0x700:
+                    impl_0x700(receivedata[i]);
+                    break;
+                case 0x737:
+                    impl_0x737(receivedata[i]);
+                    break;
+                case 0x738:
+                    impl_0x738(receivedata[i]);
+                    break;
+                case 0x739: case 0x73c: case 0x73f: case 0x742: case 0x745: case 0x748: case 0x74b: case 0x74e: case 0x751: case 0x754: case 0x757: case 0x75a: case 0x75d: case 0x760: case 0x763:
+                    a_tmp = impl_0x739(receivedata[i]);
+                    memcpy(&(ServiceCarStatus.obstacleStatusA[obs_a_count]), &a_tmp, sizeof(obstacle_data_a));
+                    obs_a_count++;
+                    break;
+                case 0x73a: case 0x73d: case 0x740: case 0x743: case 0x746: case 0x749: case 0x74c: case 0x74f: case 0x752: case 0x755: case 0x758: case 0x75b: case 0x75e: case 0x761: case 0x764:
+                    b_tmp = impl_0x73a(receivedata[i]);
+                    memcpy(&(ServiceCarStatus.obstacleStatusB[obs_b_count]), &b_tmp, sizeof(obstacle_data_b));
+                    obs_b_count++;
+                    break;
+                case 0x73b: case 0x73e: case 0x741: case 0x744: case 0x747: case 0x74a: case 0x74d: case 0x750: case 0x753: case 0x756: case 0x759: case 0x75c: case 0x75f: case 0x762: case 0x765:
+                    c_tmp = impl_0x73b(receivedata[i]);
+                    memcpy(&(ServiceCarStatus.obstacleStatusC[obs_a_count]), &c_tmp, sizeof(obstacle_data_c));
+                    obs_c_count++;
+                    break;
+                case 0x669:
+                    impl_0x669(receivedata[i]);
+                    break;
+                case 0x766:
+                    impl_0x766(receivedata[i]);
+                    break;
+                case 0x767:
+                    impl_0x767(receivedata[i]);
+                    break;
+                case 0x768:
+                    impl_0x768(receivedata[i]);
+                    break;
+                case 0x769:
+                    impl_0x769(receivedata[i]);
+                    break;
+                case 0x76c: case 0x76e: case 0x771: case 0x773: case 0x775: case 0x777: case 0x779: case 0x77b:
+                    if (receivedata[i].ID == 0x76c || receivedata[i].ID == 0x771 || receivedata[i].ID == 0x775 || receivedata[i].ID == 0x779)
+                    {
+                        left_tmp_a = impl_0x76c(receivedata[i]);
+                        memcpy(&(ServiceCarStatus.nextLaneA_left[left_lane_a_count]), &left_tmp_a, sizeof(next_lane_a));
+                        left_lane_a_count++;
+                    }
+                    else
+                    {
+                        right_tmp_a = impl_0x76c(receivedata[i]);
+                        memcpy(&(ServiceCarStatus.nextLaneA_right[right_lane_a_count]), &right_tmp_a, sizeof(next_lane_a));
+                        right_lane_a_count++;
+                    }
+                    //impl_0x76c(receivedata[i]);
+                    break;
+                case 0x76d: case 0x76f: case 0x772: case 0x774: case 0x776: case 0x778: case 0x77a: case 0x77c:
+                    if (receivedata[i].ID == 0x76d || receivedata[i].ID == 0x772 || receivedata[i].ID == 0x776 || receivedata[i].ID == 0x77a)
+                    {
+                        left_tmp_b = impl_0x76d(receivedata[i]);
+                        memcpy(&(ServiceCarStatus.nextLaneB_left[left_lane_b_count]), &left_tmp_b, sizeof(next_lane_b));
+                        left_lane_b_count++;
+                    }
+                    else
+                    {
+                        right_tmp_b = impl_0x76d(receivedata[i]);
+                        memcpy(&(ServiceCarStatus.nextLaneB_right[right_lane_b_count]), &right_tmp_b, sizeof(next_lane_b));
+                        right_lane_b_count++;
+                    }
+                    break;
+                case 0x76a:
+                    impl_0x76a(receivedata[i]);
+                    break;
+                case 0x76b:
+                    impl_0x76b(receivedata[i]);
+                    break;
+                default:
+                    break;
+            }
+        }
+       // obs_print(ServiceCarStatus.obstacleStatusA, ServiceCarStatus.obstacleStatusB, ServiceCarStatus.obstacleStatusC);
+    }
+}
+#endif
+
+//void iv::control::CanUtil::send() {
+	
+//    VCI_CAN_OBJ senddata;
+
+//	QTime time;
+//    int nTimeLastSend1;
+//	time.start();
+//    nTimeLastSend1 = time.elapsed();
+//    int nSend1Int = 10;  //10ms
+
+//	senddata.SendType = m_sendtype;
+//	senddata.ExternFlag = m_frameflag;
+//    senddata.RemoteFlag = m_frameformat;
+//    ServiceControlStatus.control_cmd_.Head = 0xEB;
+
+//	while (true) {
+//        usleep(1000);
+//		if ((time.elapsed() - nTimeLastSend1) >= nSend1Int)
+//        {
+//			nTimeLastSend1 = time.elapsed();
+//			senddata.ID = ServiceControlStatus.id;
+//            senddata.DataLen = 8;
+//            ServiceControlStatus.control_cmd_.CommunicationCount++;
+//            ServiceControlStatus.control_cmd_.CheckSum = (ServiceControlStatus.control_cmd_.Acceleration_frac + ServiceControlStatus.control_cmd_.Acceleration_int + ServiceControlStatus.control_cmd_.CommunicationCount + ServiceControlStatus.control_cmd_.Control + ServiceControlStatus.control_cmd_.Wheel_Angel[0] + ServiceControlStatus.control_cmd_.Wheel_Angel[1])%256;
+//            memcpy(senddata.Data, &ServiceControlStatus.control_cmd_.Head, 8);
+//            VCI_Transmit(m_devtype, m_devind, m_cannum, &senddata, 1);
+//            ServiceControlStatus.control_cmd_.Control &= 0xCB;
+//        }
+//	}
+//}
+
+
+void iv::control::CanUtil::startsend(bool start_or_stop)
+{
+    if(start_or_stop == true && send_connect == true && is_repeat == false)
+    {
+        is_repeat = true;
+        thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this)));			//启动发送线程
+    }
+    else if(start_or_stop == false && send_connect == true && is_repeat == true)
+    {
+        thread_send->interrupt();
+        thread_send->join();
+        is_repeat = false;
+    }
+}
+
+bool iv::control::CanUtil::did_radar_ok()
+{
+    bool temp = obs_find_flag;
+    obs_find_flag = false;
+    return temp;
+}

src/decition/common/control/can.h

@@ -1,138 +1,138 @@
-#pragma once
-#ifndef _IV_CONTROL_CAN_
-#define _IV_CONTROL_CAN_
-#include <control/controlcan.h>
-#include <common/boost.h>
-#include <common/car_status.h>
-#include <boost/serialization/singleton.hpp>
-#include <common/obstacle_type.h>
-#include <control/mobileye.h>
-#ifdef WIN32
-#include <control/can_card.h>
-#endif
-
-//#define USE_MOBILEYE
-
-namespace iv {
-    namespace control {
-        class CanUtil : public boost::noncopyable
-        {
-        public:
-            CanUtil();
-            ~CanUtil();
-
-            void openCanCard();
-            void closeCanCard();
-            void receive();
-            void send();
-
-            void initial();
-            void pack_command();
-
-            void receive_radar();	//毫米波雷达接收数据线程执行方法
-
-#ifdef USE_MOBILEYE
-            void receive_mobileye();
-#endif
-
-            /* 获取毫米波雷达传输数据*/
-            void getRadarData(iv::ObsRadar & obs_radar);
-            //zsl 2018.06.19
-            void Ui_GetRadarDate(iv::ObsRadar & obs_radar);
-        //	void Ui_GetRadarDate(iv::ObsRadarPointer & obs_radar);
-
-            bool did_radar_ok();
-            void startsend(bool start_or_stop);
-            void set_trumpet_on();
-            void set_trumpet_off();
-            void set_handbrake_on();
-            void set_handbrake_off();
-
-            void impl_0x700(VCI_CAN_OBJ receivedata);
-            void impl_0x737(VCI_CAN_OBJ receivedata);
-            void impl_0x738(VCI_CAN_OBJ receivedata);
-            obstacle_data_a impl_0x739(VCI_CAN_OBJ receivedata);
-            obstacle_data_b impl_0x73a(VCI_CAN_OBJ receivedata);
-            obstacle_data_c impl_0x73b(VCI_CAN_OBJ receivedata);
-            void impl_0x669(VCI_CAN_OBJ receivedata);
-            void impl_0x766(VCI_CAN_OBJ receivedata);
-            void impl_0x767(VCI_CAN_OBJ receivedata);
-            void impl_0x768(VCI_CAN_OBJ receivedata);
-            void impl_0x769(VCI_CAN_OBJ receivedata);
-            next_lane_a impl_0x76c(VCI_CAN_OBJ receivedata);
-            next_lane_b impl_0x76d(VCI_CAN_OBJ receivedata);
-            void impl_0x76a(VCI_CAN_OBJ receivedata);
-            void impl_0x76b(VCI_CAN_OBJ receivedata);
-
-            void obs_print(std::vector<obstacle_data_a> obstacleStatusA, std::vector<obstacle_data_b> obstacleStatusB, std::vector<obstacle_data_c> obstacleStatusC);
-
-            bool is_stop = false;
-            bool is_trumpet_duang = false;
-            bool remotecontrol_is_stop = false;
-            bool remotecontrol_is_start = false;
-        private:
-            boost::shared_ptr<boost::thread> thread_receive;	//接收控制板数据线程
-            boost::shared_ptr<boost::thread> thread_radar_receive;	//接收毫米波雷达数据线程
-
-            boost::shared_ptr<boost::thread> thread_mobileye_receive;   //mobileye
-
-            boost::shared_ptr<boost::thread> thread_send;		//发送数据线程 每10毫秒发送一次
-            iv::ObsRadar obs_radar1;
-            iv::ObsRadar obs_radar_;		//毫米波雷达障碍物数据
-            iv::ObsRadar obs_radar_ui;		//专门用来给GUI读毫米波雷达的数据
-            bool obs_find_flag = false;		//采集内容标志位，有内容就是true
-
-
-            boost::mutex mutex_;
-#ifdef WIN32
-            HINSTANCE  m_hDLL;//用来保存打开的动态库句柄
-
-
-                        LPVCI_OpenDevice VCI_OpenDevice; //接口函数指针
-                        LPVCI_CloseDevice VCI_CloseDevice;
-                        LPVCI_InitCan VCI_InitCAN;
-                        LPVCI_ReadBoardInfo VCI_ReadBoardInfo;
-                        LPVCI_ReadErrInfo VCI_ReadErrInfo;
-                        LPVCI_ReadCanStatus VCI_ReadCanStatus;
-                        LPVCI_GetReference VCI_GetReference;
-                        LPVCI_SetReference VCI_SetReference;
-                        LPVCI_GetReceiveNum VCI_GetReceiveNum;
-                        LPVCI_ClearBuffer VCI_ClearBuffer;
-                        LPVCI_StartCAN VCI_StartCAN;
-                        LPVCI_ResetCAN VCI_ResetCAN;
-                        LPVCI_Transmit VCI_Transmit;
-                        LPVCI_Receive VCI_Receive;
-                        LPVCI_GetReference2 VCI_GetReference2;
-                        LPVCI_SetReference2 VCI_SetReference2;
-                        LPVCI_ResumeConfig VCI_ResumeConfig;
-                        LPVCI_ConnectDevice VCI_ConnectDevice;
-                        LPVCI_UsbDeviceReset VCI_UsbDeviceReset;
-                        LPVCI_FindUsbDevice VCI_FindUsbDevice;
-#endif
-
-
-
-            DWORD m_devtype = 4;//USBCAN2类型号
-            DWORD m_devind = 0;		//can卡设备号  第一个插入的是0  第二个是1
-            DWORD m_cannum = 0;		//can通道号
-            DWORD m_cannum_radar = 1;	//毫米波雷达can通道号
-
-#ifdef USE_MOBILEYE
-             DWORD m_devindmob = 1;
-#endif
-
-            BYTE m_sendtype = 0;
-            BYTE m_frameflag = 0;
-            BYTE m_frameformat = 0;
-
-            bool send_connect = false;
-            bool is_repeat = false;
-            int m_connect = 0;
-
-        };
-        typedef boost::serialization::singleton<iv::control::CanUtil> CanUtilSingleton;
-    }
-}
-
-#define ServiceCanUtil iv::control::CanUtilSingleton::get_mutable_instance()
-#endif // !_IV_CONTROL_CAN_
+#pragma once
+#ifndef _IV_CONTROL_CAN_
+#define _IV_CONTROL_CAN_
+#include <control/controlcan.h>
+#include <common/boost.h>
+#include <common/car_status.h>
+#include <boost/serialization/singleton.hpp>
+#include <common/obstacle_type.h>
+#include <control/mobileye.h>
+#ifdef WIN32
+#include <control/can_card.h>
+#endif
+
+//#define USE_MOBILEYE
+
+namespace iv {
+    namespace control {
+        class CanUtil : public boost::noncopyable
+        {
+        public:
+            CanUtil();
+            ~CanUtil();
+
+            void openCanCard();
+            void closeCanCard();
+            void receive();
+            void send();
+
+            void initial();
+            void pack_command();
+
+            void receive_radar();	//毫米波雷达接收数据线程执行方法
+
+#ifdef USE_MOBILEYE
+            void receive_mobileye();
+#endif
+
+            /* 获取毫米波雷达传输数据*/
+            void getRadarData(iv::ObsRadar & obs_radar);
+            //zsl 2018.06.19
+            void Ui_GetRadarDate(iv::ObsRadar & obs_radar);
+        //	void Ui_GetRadarDate(iv::ObsRadarPointer & obs_radar);
+
+            bool did_radar_ok();
+            void startsend(bool start_or_stop);
+            void set_trumpet_on();
+            void set_trumpet_off();
+            void set_handbrake_on();
+            void set_handbrake_off();
+
+            void impl_0x700(VCI_CAN_OBJ receivedata);
+            void impl_0x737(VCI_CAN_OBJ receivedata);
+            void impl_0x738(VCI_CAN_OBJ receivedata);
+            obstacle_data_a impl_0x739(VCI_CAN_OBJ receivedata);
+            obstacle_data_b impl_0x73a(VCI_CAN_OBJ receivedata);
+            obstacle_data_c impl_0x73b(VCI_CAN_OBJ receivedata);
+            void impl_0x669(VCI_CAN_OBJ receivedata);
+            void impl_0x766(VCI_CAN_OBJ receivedata);
+            void impl_0x767(VCI_CAN_OBJ receivedata);
+            void impl_0x768(VCI_CAN_OBJ receivedata);
+            void impl_0x769(VCI_CAN_OBJ receivedata);
+            next_lane_a impl_0x76c(VCI_CAN_OBJ receivedata);
+            next_lane_b impl_0x76d(VCI_CAN_OBJ receivedata);
+            void impl_0x76a(VCI_CAN_OBJ receivedata);
+            void impl_0x76b(VCI_CAN_OBJ receivedata);
+
+            void obs_print(std::vector<obstacle_data_a> obstacleStatusA, std::vector<obstacle_data_b> obstacleStatusB, std::vector<obstacle_data_c> obstacleStatusC);
+
+            bool is_stop = false;
+            bool is_trumpet_duang = false;
+            bool remotecontrol_is_stop = false;
+            bool remotecontrol_is_start = false;
+        private:
+            boost::shared_ptr<boost::thread> thread_receive;	//接收控制板数据线程
+            boost::shared_ptr<boost::thread> thread_radar_receive;	//接收毫米波雷达数据线程
+
+            boost::shared_ptr<boost::thread> thread_mobileye_receive;   //mobileye
+
+            boost::shared_ptr<boost::thread> thread_send;		//发送数据线程 每10毫秒发送一次
+            iv::ObsRadar obs_radar1;
+            iv::ObsRadar obs_radar_;		//毫米波雷达障碍物数据
+            iv::ObsRadar obs_radar_ui;		//专门用来给GUI读毫米波雷达的数据
+            bool obs_find_flag = false;		//采集内容标志位，有内容就是true
+
+
+            boost::mutex mutex_;
+#ifdef WIN32
+            HINSTANCE  m_hDLL;//用来保存打开的动态库句柄
+
+
+                        LPVCI_OpenDevice VCI_OpenDevice; //接口函数指针
+                        LPVCI_CloseDevice VCI_CloseDevice;
+                        LPVCI_InitCan VCI_InitCAN;
+                        LPVCI_ReadBoardInfo VCI_ReadBoardInfo;
+                        LPVCI_ReadErrInfo VCI_ReadErrInfo;
+                        LPVCI_ReadCanStatus VCI_ReadCanStatus;
+                        LPVCI_GetReference VCI_GetReference;
+                        LPVCI_SetReference VCI_SetReference;
+                        LPVCI_GetReceiveNum VCI_GetReceiveNum;
+                        LPVCI_ClearBuffer VCI_ClearBuffer;
+                        LPVCI_StartCAN VCI_StartCAN;
+                        LPVCI_ResetCAN VCI_ResetCAN;
+                        LPVCI_Transmit VCI_Transmit;
+                        LPVCI_Receive VCI_Receive;
+                        LPVCI_GetReference2 VCI_GetReference2;
+                        LPVCI_SetReference2 VCI_SetReference2;
+                        LPVCI_ResumeConfig VCI_ResumeConfig;
+                        LPVCI_ConnectDevice VCI_ConnectDevice;
+                        LPVCI_UsbDeviceReset VCI_UsbDeviceReset;
+                        LPVCI_FindUsbDevice VCI_FindUsbDevice;
+#endif
+
+
+
+            DWORD m_devtype = 4;//USBCAN2类型号
+            DWORD m_devind = 0;		//can卡设备号  第一个插入的是0  第二个是1
+            DWORD m_cannum = 0;		//can通道号
+            DWORD m_cannum_radar = 1;	//毫米波雷达can通道号
+
+#ifdef USE_MOBILEYE
+             DWORD m_devindmob = 1;
+#endif
+
+            BYTE m_sendtype = 0;
+            BYTE m_frameflag = 0;
+            BYTE m_frameformat = 0;
+
+            bool send_connect = false;
+            bool is_repeat = false;
+            int m_connect = 0;
+
+        };
+        typedef boost::serialization::singleton<iv::control::CanUtil> CanUtilSingleton;
+    }
+}
+
+#define ServiceCanUtil iv::control::CanUtilSingleton::get_mutable_instance()
+#endif // !_IV_CONTROL_CAN_

src/decition/common/control/can_card.h

@@ -1,111 +1,111 @@
-#include <control/control_status.h>
-
-#if 1
-void iv::control::ControlStatus::set_accelerate(float percent)
-{
-    command.bit.acce = (unsigned)((percent + 7) * 20);
-}
-void iv::control::ControlStatus::set_wheel_angle(float angle)
-{
-    command.bit.ang_H = int(angle) * 10 >> 8;
-
-    command.bit.ang_L = int(angle) * 10 % 256;
-}
-void iv::control::ControlStatus::set_engine(char enable)
-{
-    command.bit.engine = enable;
-}
-void iv::control::ControlStatus::set_door(char enable){
-    command.bit.door = enable;
-}
-void iv::control::ControlStatus::set_speaker(bool enable){
-    if (enable == true)
-        command.bit.speaker = 1;
-    else
-        command.bit.speaker = 0;
-}
-void iv::control::ControlStatus::set_flicker(bool enable){
-    if (enable == true)
-        command.bit.flicker = 1;
-    else
-        command.bit.flicker = 0;
-}
-void iv::control::ControlStatus::set_light(bool enable){
-    if (enable == true)
-       command.bit.bright = 1;
-    else
-       command.bit.bright = 0;
-}
-void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
-    if (left == true)
-    {
-        command.bit.left_turn = 1;
-        command.bit.right_turn = 0;
-    }
-    else if (right == true)
-    {
-        command.bit.left_turn = 0;
-        command.bit.right_turn = 1;
-    }
-    else
-    {
-        command.bit.left_turn = 0;
-        command.bit.right_turn = 0;
-    }
-}
-
-#else
-
-void iv::control::ControlStatus::set_accelerate(float percent)
-{
-    command.bit.acce = (unsigned)((percent + 7) * 20);
-}
-void iv::control::ControlStatus::set_wheel_angle(float angle)
-{
-    command.bit.ang_H = angle * 10 >> 8;
-
-    command.bit.ang_L = angle * 10 % 256;
-}
-void iv::control::ControlStatus::set_engine(char enable)
-{
-    command.bit.engine = enable;
-}
-void iv::control::ControlStatus::set_door(char enable){
-    command.bit.door = enable;
-}
-void iv::control::ControlStatus::set_speaker(bool enable){
-    if (enable == true)
-        command.bit.speaker = 1;
-    else
-        command.bit.speaker = 0;
-}
-void iv::control::ControlStatus::set_flicker(bool enable){
-    if (enable == true)
-        command.bit.flicker = 1;
-    else
-        command.bit.flicker = 0;
-}
-void iv::control::ControlStatus::set_light(bool enable){
-    if (enable == true)
-       command.bit.bright = 1;
-    else
-       command.bit.bright = 0;
-}
-void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
-    if (left == true)
-    {
-        command.bit.left_turn = 1;
-        command.bit.right_turn = 0;
-    }
-    else if (right == true)
-    {
-        command.bit.left_turn = 0;
-        command.bit.right_turn = 1;
-    }
-    else
-    {
-        command.bit.left_turn = 0;
-        command.bit.right_turn = 0;
-    }
-}
-#endif
+#include <control/control_status.h>
+
+#if 1
+void iv::control::ControlStatus::set_accelerate(float percent)
+{
+    command.bit.acce = (unsigned)((percent + 7) * 20);
+}
+void iv::control::ControlStatus::set_wheel_angle(float angle)
+{
+    command.bit.ang_H = int(angle) * 10 >> 8;
+
+    command.bit.ang_L = int(angle) * 10 % 256;
+}
+void iv::control::ControlStatus::set_engine(char enable)
+{
+    command.bit.engine = enable;
+}
+void iv::control::ControlStatus::set_door(char enable){
+    command.bit.door = enable;
+}
+void iv::control::ControlStatus::set_speaker(bool enable){
+    if (enable == true)
+        command.bit.speaker = 1;
+    else
+        command.bit.speaker = 0;
+}
+void iv::control::ControlStatus::set_flicker(bool enable){
+    if (enable == true)
+        command.bit.flicker = 1;
+    else
+        command.bit.flicker = 0;
+}
+void iv::control::ControlStatus::set_light(bool enable){
+    if (enable == true)
+       command.bit.bright = 1;
+    else
+       command.bit.bright = 0;
+}
+void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
+    if (left == true)
+    {
+        command.bit.left_turn = 1;
+        command.bit.right_turn = 0;
+    }
+    else if (right == true)
+    {
+        command.bit.left_turn = 0;
+        command.bit.right_turn = 1;
+    }
+    else
+    {
+        command.bit.left_turn = 0;
+        command.bit.right_turn = 0;
+    }
+}
+
+#else
+
+void iv::control::ControlStatus::set_accelerate(float percent)
+{
+    command.bit.acce = (unsigned)((percent + 7) * 20);
+}
+void iv::control::ControlStatus::set_wheel_angle(float angle)
+{
+    command.bit.ang_H = angle * 10 >> 8;
+
+    command.bit.ang_L = angle * 10 % 256;
+}
+void iv::control::ControlStatus::set_engine(char enable)
+{
+    command.bit.engine = enable;
+}
+void iv::control::ControlStatus::set_door(char enable){
+    command.bit.door = enable;
+}
+void iv::control::ControlStatus::set_speaker(bool enable){
+    if (enable == true)
+        command.bit.speaker = 1;
+    else
+        command.bit.speaker = 0;
+}
+void iv::control::ControlStatus::set_flicker(bool enable){
+    if (enable == true)
+        command.bit.flicker = 1;
+    else
+        command.bit.flicker = 0;
+}
+void iv::control::ControlStatus::set_light(bool enable){
+    if (enable == true)
+       command.bit.bright = 1;
+    else
+       command.bit.bright = 0;
+}
+void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
+    if (left == true)
+    {
+        command.bit.left_turn = 1;
+        command.bit.right_turn = 0;
+    }
+    else if (right == true)
+    {
+        command.bit.left_turn = 0;
+        command.bit.right_turn = 1;
+    }
+    else
+    {
+        command.bit.left_turn = 0;
+        command.bit.right_turn = 0;
+    }
+}
+#endif

src/decition/common/control/control_status.h

@@ -1,47 +1,47 @@
-#pragma once
-//由于控制器指令共享同一个ID 0x20，建立此类维护控制指令的最新状态
-
-#include <common/boost.h>
-#include <cstdint>
-#include <boost/serialization/singleton.hpp>
-#include <control/vv7.h>
-
-namespace iv {
-    namespace control {
-        class ControlStatus : public boost::noncopyable
-        {
-        public:
-            /*****************
-             * ****测试标志位*****
-             * ***************/
-            int normal_speed  = 0;//常规速度
-            int swerve_speed  = 0;//转弯速度
-            int high_speed = 0;//快速
-            int mid_speed = 0;//中速
-            int low_speed = 0;//慢速
-            int change_line  = -1;//换道标志
-            int stop_obstacle = -1;//停障标志
-            int elude_obstacle = -1;//避障标志
-            int special_signle = -1;//特殊信号标志
-            int car_pullover = -1;//靠边停车标志位
-
-            Command command;
-
-            int command_ID = 0x12;
-
-            void set_accelerate(float percent);
-
-            void set_wheel_angle(float angle);
-
-            void set_engine(char enable);
-            void set_door(char enable);
-            void set_speaker(bool enable);
-            void set_flicker(bool enable);
-            void set_light(bool enable);
-            void set_turnsignals_control(bool left, bool right);
-
-        };
-        typedef boost::serialization::singleton<iv::control::ControlStatus> ControlStatusSingleton;
-    }
-}
-#define ServiceControlStatus iv::control::ControlStatusSingleton::get_mutable_instance()
+#pragma once
+//由于控制器指令共享同一个ID 0x20，建立此类维护控制指令的最新状态
+
+#include <common/boost.h>
+#include <cstdint>
+#include <boost/serialization/singleton.hpp>
+#include <control/vv7.h>
+
+namespace iv {
+    namespace control {
+        class ControlStatus : public boost::noncopyable
+        {
+        public:
+            /*****************
+             * ****测试标志位*****
+             * ***************/
+            int normal_speed  = 0;//常规速度
+            int swerve_speed  = 0;//转弯速度
+            int high_speed = 0;//快速
+            int mid_speed = 0;//中速
+            int low_speed = 0;//慢速
+            int change_line  = -1;//换道标志
+            int stop_obstacle = -1;//停障标志
+            int elude_obstacle = -1;//避障标志
+            int special_signle = -1;//特殊信号标志
+            int car_pullover = -1;//靠边停车标志位
+
+            Command command;
+
+            int command_ID = 0x12;
+
+            void set_accelerate(float percent);
+
+            void set_wheel_angle(float angle);
+
+            void set_engine(char enable);
+            void set_door(char enable);
+            void set_speaker(bool enable);
+            void set_flicker(bool enable);
+            void set_light(bool enable);
+            void set_turnsignals_control(bool left, bool right);
+
+        };
+        typedef boost::serialization::singleton<iv::control::ControlStatus> ControlStatusSingleton;
+    }
+}
+#define ServiceControlStatus iv::control::ControlStatusSingleton::get_mutable_instance()

src/decition/common/control/controlcan.h

@@ -1,48 +1,48 @@
-#include <control/controller.h>
-
-iv::control::Controller::Controller() {
-
-}
-
-iv::control::Controller::~Controller() {
-
-}
-
-void iv::control::Controller::inialize() {
-
-}
-
-void iv::control::Controller::control_accelerate(float percent) {
-    ServiceControlStatus.set_accelerate(percent);
-}
-
-void iv::control::Controller::control_wheel(float angle) {
-    ServiceControlStatus.set_wheel_angle(angle);
-}
-
-void iv::control::Controller::control_turnsignals(bool left, bool right){
-    ServiceControlStatus.set_turnsignals_control(left,right);
-}
-
-void iv::control::Controller::control_engine(char enable){
-    ServiceControlStatus.set_engine(enable);
-}
-
-void iv::control::Controller::control_door(char enable){
-    ServiceControlStatus.set_door(enable);
-}
-
-void iv::control::Controller::control_speaker(bool enable){
-    ServiceControlStatus.set_speaker(enable);
-}
-
-void iv::control::Controller::control_light(bool enable){
-    ServiceControlStatus.set_light(enable);
-}
-
-void iv::control::Controller::control_flicker(bool enable){
-    ServiceControlStatus.set_flicker(enable);
-}
-void iv::control::Controller::control_braking(float percent){
-
-}
+#include <control/controller.h>
+
+iv::control::Controller::Controller() {
+
+}
+
+iv::control::Controller::~Controller() {
+
+}
+
+void iv::control::Controller::inialize() {
+
+}
+
+void iv::control::Controller::control_accelerate(float percent) {
+    ServiceControlStatus.set_accelerate(percent);
+}
+
+void iv::control::Controller::control_wheel(float angle) {
+    ServiceControlStatus.set_wheel_angle(angle);
+}
+
+void iv::control::Controller::control_turnsignals(bool left, bool right){
+    ServiceControlStatus.set_turnsignals_control(left,right);
+}
+
+void iv::control::Controller::control_engine(char enable){
+    ServiceControlStatus.set_engine(enable);
+}
+
+void iv::control::Controller::control_door(char enable){
+    ServiceControlStatus.set_door(enable);
+}
+
+void iv::control::Controller::control_speaker(bool enable){
+    ServiceControlStatus.set_speaker(enable);
+}
+
+void iv::control::Controller::control_light(bool enable){
+    ServiceControlStatus.set_light(enable);
+}
+
+void iv::control::Controller::control_flicker(bool enable){
+    ServiceControlStatus.set_flicker(enable);
+}
+void iv::control::Controller::control_braking(float percent){
+
+}

src/decition/common/control/controller.h

@@ -1,38 +1,38 @@
-#pragma once
-/*
-*控制器
-*/
-#ifndef _IV_CONTROL_CONTROLLER_
-#define _IV_CONTROL_CONTROLLER_
-
-#include <common/boost.h>
-#include <common/car_status.h>
-#include <control/control_status.h>
-
-namespace iv {
-    namespace control {
-        class Controller
-        {
-        public:
-            Controller();
-            ~Controller();
-            void inialize();// 初始化
-
-            void control_accelerate(float percet);	//油门开度控制
-            void control_wheel(float angle);		//方向盘控制
-            void control_turnsignals(bool left, bool right);
-            void control_engine(char enable);
-            void control_door(char enable);
-            void control_speaker(bool enable);
-            void control_light(bool enable);
-            void control_flicker(bool enable);
-            void control_braking(float percent);
-            ///* 获取当前车辆状态*/
-            //void getCurrentCarStatus(iv::CarStatus & car_status);
-
-        private:
-        };
-    }
-}
-
-#endif // !_IV_CONTROL_CONTROLLER_
+#pragma once
+/*
+*控制器
+*/
+#ifndef _IV_CONTROL_CONTROLLER_
+#define _IV_CONTROL_CONTROLLER_
+
+#include <common/boost.h>
+#include <common/car_status.h>
+#include <control/control_status.h>
+
+namespace iv {
+    namespace control {
+        class Controller
+        {
+        public:
+            Controller();
+            ~Controller();
+            void inialize();// 初始化
+
+            void control_accelerate(float percet);	//油门开度控制
+            void control_wheel(float angle);		//方向盘控制
+            void control_turnsignals(bool left, bool right);
+            void control_engine(char enable);
+            void control_door(char enable);
+            void control_speaker(bool enable);
+            void control_light(bool enable);
+            void control_flicker(bool enable);
+            void control_braking(float percent);
+            ///* 获取当前车辆状态*/
+            //void getCurrentCarStatus(iv::CarStatus & car_status);
+
+        private:
+        };
+    }
+}
+
+#endif // !_IV_CONTROL_CONTROLLER_

src/decition/common/control/mobileye.h

@@ -18,7 +18,7 @@ iv::Ivlog * givlog;
 
 int main(int argc, char *argv[])
 {
-    RegisterIVBackTrace();
+    //RegisterIVBackTrace();
     showversion("decition_brain");
     QCoreApplication a(argc, argv);
 

src/decition/common/perception/eyes.cpp

@@ -1,132 +1,132 @@
-#include <perception/eyes.h>
-#include <time.h>
-
-iv::perception::Eyes::Eyes() {
-    mgpsindex = 0;
-    mgpsreadindex = 0;
-
-}
-iv::perception::Eyes::~Eyes() {
-
-}
-
-void iv::perception::Eyes::inialize() {
-
-    sensor_lidar = new iv::perception::LiDARSensor();
-    sensor_radar = new iv::perception::RadarSensor();
-    sensor_camera = new iv::perception::CameraSensor();
-    sensor_gps = new iv::perception::GPSSensor();
-
-
-    sensor_lidar->registerCallBack<iv::perception::LiDARSensor::sig_cb_lidar_sensor_obstacle_grid>(boost::bind(&iv::perception::Eyes::cb_lidar_grid, this, _1));
-    //sensor_radar->registerCallBack<iv::perception::RadarSensor::sig_cb_radar_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_radar, this, _1));
-    //sensor_camera->registerCallBack<iv::perception::CameraSensor::sig_cb_camera_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_camera, this, _1));
-    sensor_gps->registerCallBack<iv::perception::GPSSensor::sig_cb_gps_sensor_data>(boost::bind(&iv::perception::Eyes::cb_gps, this, _1));
-    sensor_lidar->start();
-    sensor_radar->start();
-    sensor_camera->start();
-    sensor_gps->start();
-    obs_lidar_gridptr_ = NULL;
-    obs_camera_ = NULL;
-}
-
-void iv::perception::Eyes::cb_lidar_grid(iv::LidarGridPtr obs) {
-    mutex_.lock();
-    if (obs_lidar_gridptr_ != NULL)
-    {
-        free(obs_lidar_gridptr_);
-    }
-    obs_lidar_gridptr_ = obs;
-    obs = NULL;
-    mutex_.unlock();
-}
-
-void iv::perception::Eyes::cb_radar(iv::ObsRadar obs) {
-    boost::mutex::scoped_lock(mutex_);
-    obs_radar_ = obs;
-}
-
-void iv::perception::Eyes::cb_camera(iv::CameraInfoPtr obs) {
-    mutex_.lock();
-    //boost::mutex::scoped_lock(mutex_);
-    if (obs_camera_ != NULL)
-    {
-        free(obs_camera_);
-    }
-    obs_camera_ = obs;
-    obs = NULL;
-    mutex_.unlock();
-}
-
-void iv::perception::Eyes::cb_gps(iv::GPSData gps_data) {
-    if((gps_data->gps_lat<0.01)||(gps_data->gps_lat>90))return;
-    boost::mutex::scoped_lock(mutex_);
-//    gps_ins_data_ = gps_data;
-
-    if(gps_data == NULL)return;
-    iv::GPSData data(new iv::GPS_INS);
-    if(data == NULL)return;
-    *data = *gps_data;
-    gps_ins_data_ = data;
-    mgpsindex++;
-
-}
-
-bool iv::perception::Eyes::isAllSensorRunning() {
-    return true;
-    bool bx;
-    //bx = sensor_lidar->isRunning() && sensor_radar->isRunning() && sensor_camera->isRunning() && sensor_gps->isRunning();
-    bx = sensor_gps->isRunning();
-//	ODS("Get Running time = %d", GetTickCount() - ticks);
-    return bx;
-}
-
-void iv::perception::Eyes::stopAllSensor() {
-    sensor_lidar->stop();
-    sensor_radar->stop();
-    sensor_camera->stop();
-    sensor_gps->stop();
-}
-
-void iv::perception::Eyes::getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& brain_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr) {
-    //brain_obs_camera = NULL;
-    brain_gps_data = NULL;
-    brain_obs_radar = NULL;
-    while (true) {
-        if (mutex_.try_lock()) {
-//            ServiceLidar.copylidarobsto(brain_obs_lidar_gridptr);
-            if(obs_lidar_gridptr_ != NULL)
-            {
-                brain_obs_lidar_gridptr  = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
-                memcpy(brain_obs_lidar_gridptr,obs_lidar_gridptr_,sizeof(Obs_grid[iv::grx][iv::gry]));
-             //   brain_obs_lidar_gridptr = obs_lidar_gridptr_;
-                free(obs_lidar_gridptr_);
-                obs_lidar_gridptr_ = NULL;
-            }
-            obs_lidar_gridptr_ = NULL;
-
- //           obs_radar_.swap(brain_obs_radar);
-            brain_obs_radar = NULL;
-
-            //obs_camera_.swap(brain_obs_camera);
-            brain_obs_camera = NULL;
-
-            if(mgpsindex != mgpsreadindex)
-  //          if(gps_ins_data_ != NULL)
-            {
-                iv::GPSData data(new iv::GPS_INS);
-                *data = *gps_ins_data_;
-                brain_gps_data =  data;
-                mgpsreadindex = mgpsindex;
-            }
-
-//            brain_gps_data = gps_ins_data_;
-//            gps_ins_data_ = NULL;
-
-
-            obs_camera_ = NULL;
-            mutex_.unlock();
-            break;
-        }
-    }
-}
+#include <perception/eyes.h>
+#include <time.h>
+
+iv::perception::Eyes::Eyes() {
+    mgpsindex = 0;
+    mgpsreadindex = 0;
+
+}
+iv::perception::Eyes::~Eyes() {
+
+}
+
+void iv::perception::Eyes::inialize() {
+
+    sensor_lidar = new iv::perception::LiDARSensor();
+    sensor_radar = new iv::perception::RadarSensor();
+    sensor_camera = new iv::perception::CameraSensor();
+    sensor_gps = new iv::perception::GPSSensor();
+
+
+    sensor_lidar->registerCallBack<iv::perception::LiDARSensor::sig_cb_lidar_sensor_obstacle_grid>(boost::bind(&iv::perception::Eyes::cb_lidar_grid, this, _1));
+    //sensor_radar->registerCallBack<iv::perception::RadarSensor::sig_cb_radar_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_radar, this, _1));
+    //sensor_camera->registerCallBack<iv::perception::CameraSensor::sig_cb_camera_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_camera, this, _1));
+    sensor_gps->registerCallBack<iv::perception::GPSSensor::sig_cb_gps_sensor_data>(boost::bind(&iv::perception::Eyes::cb_gps, this, _1));
+    sensor_lidar->start();
+    sensor_radar->start();
+    sensor_camera->start();
+    sensor_gps->start();
+    obs_lidar_gridptr_ = NULL;
+    obs_camera_ = NULL;
+}
+
+void iv::perception::Eyes::cb_lidar_grid(iv::LidarGridPtr obs) {
+    mutex_.lock();
+    if (obs_lidar_gridptr_ != NULL)
+    {
+        free(obs_lidar_gridptr_);
+    }
+    obs_lidar_gridptr_ = obs;
+    obs = NULL;
+    mutex_.unlock();
+}
+
+void iv::perception::Eyes::cb_radar(iv::ObsRadar obs) {
+    boost::mutex::scoped_lock(mutex_);
+    obs_radar_ = obs;
+}
+
+void iv::perception::Eyes::cb_camera(iv::CameraInfoPtr obs) {
+    mutex_.lock();
+    //boost::mutex::scoped_lock(mutex_);
+    if (obs_camera_ != NULL)
+    {
+        free(obs_camera_);
+    }
+    obs_camera_ = obs;
+    obs = NULL;
+    mutex_.unlock();
+}
+
+void iv::perception::Eyes::cb_gps(iv::GPSData gps_data) {
+    if((gps_data->gps_lat<0.01)||(gps_data->gps_lat>90))return;
+    boost::mutex::scoped_lock(mutex_);
+//    gps_ins_data_ = gps_data;
+
+    if(gps_data == NULL)return;
+    iv::GPSData data(new iv::GPS_INS);
+    if(data == NULL)return;
+    *data = *gps_data;
+    gps_ins_data_ = data;
+    mgpsindex++;
+
+}
+
+bool iv::perception::Eyes::isAllSensorRunning() {
+    return true;
+    bool bx;
+    //bx = sensor_lidar->isRunning() && sensor_radar->isRunning() && sensor_camera->isRunning() && sensor_gps->isRunning();
+    bx = sensor_gps->isRunning();
+//	ODS("Get Running time = %d", GetTickCount() - ticks);
+    return bx;
+}
+
+void iv::perception::Eyes::stopAllSensor() {
+    sensor_lidar->stop();
+    sensor_radar->stop();
+    sensor_camera->stop();
+    sensor_gps->stop();
+}
+
+void iv::perception::Eyes::getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& brain_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr) {
+    //brain_obs_camera = NULL;
+    brain_gps_data = NULL;
+    brain_obs_radar = NULL;
+    while (true) {
+        if (mutex_.try_lock()) {
+//            ServiceLidar.copylidarobsto(brain_obs_lidar_gridptr);
+            if(obs_lidar_gridptr_ != NULL)
+            {
+                brain_obs_lidar_gridptr  = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+                memcpy(brain_obs_lidar_gridptr,obs_lidar_gridptr_,sizeof(Obs_grid[iv::grx][iv::gry]));
+             //   brain_obs_lidar_gridptr = obs_lidar_gridptr_;
+                free(obs_lidar_gridptr_);
+                obs_lidar_gridptr_ = NULL;
+            }
+            obs_lidar_gridptr_ = NULL;
+
+ //           obs_radar_.swap(brain_obs_radar);
+            brain_obs_radar = NULL;
+
+            //obs_camera_.swap(brain_obs_camera);
+            brain_obs_camera = NULL;
+
+            if(mgpsindex != mgpsreadindex)
+  //          if(gps_ins_data_ != NULL)
+            {
+                iv::GPSData data(new iv::GPS_INS);
+                *data = *gps_ins_data_;
+                brain_gps_data =  data;
+                mgpsreadindex = mgpsindex;
+            }
+
+//            brain_gps_data = gps_ins_data_;
+//            gps_ins_data_ = NULL;
+
+
+            obs_camera_ = NULL;
+            mutex_.unlock();
+            break;
+        }
+    }
+}

src/decition/common/perception/eyes.h

@@ -1,61 +1,61 @@
-#pragma once
-
-#ifndef _IV_PERCEPTION_EYES_
-#define _IV_PERCEPTION_EYES_
-
-#include <perception/sensor_lidar.h>
-#include <perception/sensor_radar.h>
-#include <perception/sensor_camera.h>
-#include <perception/sensor_gps.h>
-
-namespace iv {
-    namespace perception {
-        class Eyes
-        {
-        public:
-            Eyes();
-            ~Eyes();
-
-
-            void inialize();/* 初始化*/
-
-
-            bool isAllSensorRunning(); /* 所有传感器是否都在运行*/
-
-
-            void stopAllSensor(); /* 关闭所有传感器*/
-
-            void getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& bran_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr);
-
-            iv::ObsLiDAR obs_lidar_;
-            iv::LidarGridPtr obs_lidar_gridptr_;
-            iv::ObsRadar obs_radar_;
-            iv::CameraInfoPtr obs_camera_;
-            iv::GPSData gps_ins_data_;
-
-        private:
-
-            void cb_lidar(iv::ObsLiDAR obs);
-            void cb_lidar_grid(iv::LidarGridPtr obs);
-            void cb_radar(iv::ObsRadar obs);
-            void cb_camera(iv::CameraInfoPtr obs);
-            void cb_gps(iv::GPSData gps_data);
-
-            iv::perception::LiDARSensor* sensor_lidar;
-            iv::perception::RadarSensor* sensor_radar;
-            iv::perception::CameraSensor* sensor_camera;
-            iv::perception::GPSSensor* sensor_gps;
-
-            boost::mutex mutex_;
-
-            int ready1 = 0, ready2 = 0;
-
-            int mgpsindex;
-            int mgpsreadindex;
-        };
-
-    }
-}
-
-
-#endif // !_IV_PERCEPTION_EYES_
+#pragma once
+
+#ifndef _IV_PERCEPTION_EYES_
+#define _IV_PERCEPTION_EYES_
+
+#include <perception/sensor_lidar.h>
+#include <perception/sensor_radar.h>
+#include <perception/sensor_camera.h>
+#include <perception/sensor_gps.h>
+
+namespace iv {
+    namespace perception {
+        class Eyes
+        {
+        public:
+            Eyes();
+            ~Eyes();
+
+
+            void inialize();/* 初始化*/
+
+
+            bool isAllSensorRunning(); /* 所有传感器是否都在运行*/
+
+
+            void stopAllSensor(); /* 关闭所有传感器*/
+
+            void getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& bran_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr);
+
+            iv::ObsLiDAR obs_lidar_;
+            iv::LidarGridPtr obs_lidar_gridptr_;
+            iv::ObsRadar obs_radar_;
+            iv::CameraInfoPtr obs_camera_;
+            iv::GPSData gps_ins_data_;
+
+        private:
+
+            void cb_lidar(iv::ObsLiDAR obs);
+            void cb_lidar_grid(iv::LidarGridPtr obs);
+            void cb_radar(iv::ObsRadar obs);
+            void cb_camera(iv::CameraInfoPtr obs);
+            void cb_gps(iv::GPSData gps_data);
+
+            iv::perception::LiDARSensor* sensor_lidar;
+            iv::perception::RadarSensor* sensor_radar;
+            iv::perception::CameraSensor* sensor_camera;
+            iv::perception::GPSSensor* sensor_gps;
+
+            boost::mutex mutex_;
+
+            int ready1 = 0, ready2 = 0;
+
+            int mgpsindex;
+            int mgpsreadindex;
+        };
+
+    }
+}
+
+
+#endif // !_IV_PERCEPTION_EYES_

src/decition/common/perception/impl_camera.cpp

@@ -1,43 +1,43 @@
-/*
-* 在此方法中处理摄像头的打开、处理数据、关闭等
-* 识别到障碍物时 触发signal_camera_obstacle
-*
-*/
-#include <perception/sensor_camera.h>
-
-void iv::perception::CameraSensor::processSensor() {
-	std::cout << "camera process start" << std::endl;
-	//打开激光雷达
-	//障碍物识别
-	iv::ObsCamera obs(new std::vector<iv::ObstacleBasic>);
-	iv::ObstacleBasic t = {  };
-	obs->push_back(t);
-	//ODS("\n\n------>thread camera: %X\n\n", GetCurrentThreadId());
-	while (true) {
-		srand(time(NULL));
-		if (rand() % 11 == 0) {
-			obs->at(0).valid = true;
-			obs->at(0).nomal_x = rand() % 5;
-			obs->at(0).nomal_y = rand() / 6;
-			obs->at(0).nomal_z = rand() / 7;
-
-            //signal_camera_obstacle->operator()(obs);//触发
-
-
-			//std::cout << "Camera Dectected: " << obs->at(0).nomal_x << " " << obs->at(0).nomal_y << " " << obs->at(0).nomal_z << std::endl;
-
-		}
-		/*if (rand() % 9 == 0)
-			break;*/
-#ifdef linux
-            sleep(1);
-#endif
-#ifdef WIN32
-            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
-  //          Sleep(1000);
-#endif
-        //boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
-	}
-
-	std::cout << "camera process end" << std::endl;
-}
+/*
+* 在此方法中处理摄像头的打开、处理数据、关闭等
+* 识别到障碍物时 触发signal_camera_obstacle
+*
+*/
+#include <perception/sensor_camera.h>
+
+void iv::perception::CameraSensor::processSensor() {
+	std::cout << "camera process start" << std::endl;
+	//打开激光雷达
+	//障碍物识别
+	iv::ObsCamera obs(new std::vector<iv::ObstacleBasic>);
+	iv::ObstacleBasic t = {  };
+	obs->push_back(t);
+	//ODS("\n\n------>thread camera: %X\n\n", GetCurrentThreadId());
+	while (true) {
+		srand(time(NULL));
+		if (rand() % 11 == 0) {
+			obs->at(0).valid = true;
+			obs->at(0).nomal_x = rand() % 5;
+			obs->at(0).nomal_y = rand() / 6;
+			obs->at(0).nomal_z = rand() / 7;
+
+            //signal_camera_obstacle->operator()(obs);//触发
+
+
+			//std::cout << "Camera Dectected: " << obs->at(0).nomal_x << " " << obs->at(0).nomal_y << " " << obs->at(0).nomal_z << std::endl;
+
+		}
+		/*if (rand() % 9 == 0)
+			break;*/
+#ifdef linux
+            sleep(1);
+#endif
+#ifdef WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+  //          Sleep(1000);
+#endif
+        //boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+	}
+
+	std::cout << "camera process end" << std::endl;
+}

src/decition/common/perception/impl_gps.cpp

@@ -1,346 +1,346 @@
-#include <perception/sensor_gps.h>
-#include <decition/gnss_coordinate_convert.h>
-#include <control/can.h>
-#include <QDebug>
-#include <QNetworkDatagram>
-
-static double   cast_8_byte_to_double(const uint8_t *b);
-static float    cast_4_byte_to_float(const uint8_t *b);
-static  int32_t cast_3_byte_to_int32(const uint8_t *b);
-
-void iv::perception::GPSSensor::processSensor()
-{
-
-    return;
-	//todo  GPS/惯导 设备接口 对接
-    /*Initialize udp server, listen on port 3000.*/
-    /*int sockfd;
-    struct sockaddr_in addr;
-    socklen_t addrlen;
-    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
-    if (sockfd < 0)
-    {
-       printf("socket failed\n");
-       exit(EXIT_FAILURE);
-    }
-    addrlen = sizeof(struct sockaddr_in);
-    bzero(&addr, addrlen);
-    addr.sin_family = AF_INET;
-    addr.sin_addr.s_addr = htonl(INADDR_ANY);
-    addr.sin_port = htons(3000);
-    if (bind(sockfd, (struct sockaddr*)(&addr), addrlen) < 0)
-    {
-       printf("bind fail\n");
-       exit(EXIT_FAILURE);
-    }*/
-    QUdpSocket *udpsocket;//summer
-    udpsocket = new QUdpSocket();
-    udpsocket->bind(QHostAddress::Any, 3000);
- //   udpsocket->bind(3000);
- /*   if(!udpsocket->waitForConnected())
-    {
-        printf("bind fail\n");
-        exit(EXIT_FAILURE);
-    }*/
-    char *Buf = new char[100] ;//summer unsigned char - char
-    memset(Buf,0,100);
-    unsigned char recvBuf[100] = {0};
-
-	/*UDP initialization done. Next prepare for receiving data.*/
-
-	iv::GPSData data(new iv::GPS_INS);
-	int x, y, z;
-
-	//iv::CarStatus currentCarStatus;
-	//ODS("\n\n\n------>thread gps: %X\n\n\n\n", GetCurrentThreadId());
-	while (true) {
-        int rec = 0;
-        if(udpsocket->hasPendingDatagrams())
-        {
-  //          std::cout<<"have data."<<std::endl;
-            QNetworkDatagram datagram = udpsocket->receiveDatagram();
-            QByteArray ba = datagram.data();
-            if(ba.size() != NOUTPUT_PACKET_LENGTH)
-            {
-                continue;
-            }
-            else
-            {
-                rec = NOUTPUT_PACKET_LENGTH;
-                memcpy(recvBuf,ba.data(),NOUTPUT_PACKET_LENGTH);
-            }
-            datagram.clear();
-        }
-        else
-        {
-#ifdef Q_OS_LINUX
-            usleep(1000);
-#endif
-#ifdef WIN32
-            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
-//            Sleep(5);
-#endif
-            continue;
-//            std::cout<<"running."<<std::endl;
- //           std::this_thread::sleep_for(std::chrono::milliseconds(1));
-        }
-/*        if(udpsocket->waitForReadyRead())
-            rec = udpsocket->read(Buf,100);
-        //recvBuf = (unsigned char*)Buf;
-        convertStrToUnChar(Buf,recvBuf);//summer */
-        //int rec = recvfrom(sockfd, recvBuf, 100, 0, (struct sockaddr *)(&addr), &addrlen);
-		/*Check the receiving data*/
-		//There must be 72 bytes in one package.
-		if (rec != NOUTPUT_PACKET_LENGTH) {
-			printf("ERR: rec must be 72 bytes\n");
-			continue;
-		}
-
-		if (recvBuf[PI_SYNC] != NCOM_SYNC) {
-			printf("ERR: head always be 0xE7\n");
-			continue;
-		}
-
-        data->ins_status = recvBuf[PI_INS_NAV_MODE];
-        if(data->ins_status == 0x0B)continue;
-
-        //31.1150882 121.2211320 356.9
-        //31.1150844 121.2210996 356.9 VV7_3
-
-		/*  Start decoding
-		Details in ncomman.pdf
-		Byte		| 0	   | 1-20	| 21         | 22      | 23-60  | 61      | 62       | 63-70  | 71
-		Description	| Sync | BatchA | NaviStatus | ChkSum1 | BatchB | ChkSum2 | StatusCh | BatchS | ChkSum3
-		*/
-
-		//start decoding -> Batch A
-        //Time is transmitted as milliseconds into the current GPS minute. The range is 0–59, 999 ms.
-
-		/*
-		Acceleration x is the host object’s acceleration in the x-direction
-		(i.e.after the IMU to host attitude matrix has been applied).
-		It is a signed word in units of 1 × 10^(−4)  m / (s^2)
-		*/
-		x = cast_3_byte_to_int32(recvBuf + PI_ACCEL_X);
-		y = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Y);
-		z = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Z);
-		if (x != INV_INT_24) data->accel_x = x * ACC2MPS2;
-		if (y != INV_INT_24) data->accel_y = y * ACC2MPS2;
-		if (z != INV_INT_24) data->accel_z = z * ACC2MPS2;
-
-		/*
-		Angular rate x is the host object’s angular rate about its x-axis
-		(i.e. after the IMU to host attitude matrix has been applied).
-		It is a signed word in units of 1 × 10−5 radians/s
-		*/
-		x = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_X);
-		y = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Y);
-		z = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Z);
-		if (x != INV_INT_24) data->ang_rate_x = x * (RATE2RPS * RAD2DEG);
-		if (y != INV_INT_24) data->ang_rate_y = y * (RATE2RPS * RAD2DEG);
-		if (z != INV_INT_24) data->ang_rate_z = z * (RATE2RPS * RAD2DEG);
-
-		/*
-		The navigation status byte value should be 0–7, 10 or 20–22 to be valid
-		for customer use. See page 10. A value of 11 indicates the packet
-		follows NCOM structure-B and should be ignored.
-		*/
-		data->ins_status = recvBuf[PI_INS_NAV_MODE];
-		//navi_status refer to imu status 2 3 4.
-		//check this out in page 11 table 5.
-
-		/*
-		Checksum 1 allows the software to verify the integrity of bytes 1–21.
-		The sync byte is ignored. In low-latency applications the inertial
-		measurements in Batch A can be used to update a previous solution
-		without waiting for the rest of the packet to be received. Contact Oxford
-		Technical Solutions for source code to perform this function.
-        */
-        //31.1150796 121.2211047 355.8
-
-		//start decoding Batch B
-		/*
-		Position, orientation and velocity output. See Table 6, for a detailed
-		description.
-		*/
-		/*Byte23-30. The Latitude of the INS. It is a double in units of radians.*/
-		/*Byte31-38. Longitude of the INS. It is a double in units of radians.*/
-		/*Byte39-42. Altitude of the INS. It is a float in units of metres.*/
-		/*double latitude;
-		double longitude;
-		memcpy(&latitude, recvBuf + 23, sizeof(double));
-		memcpy(&longitude, recvBuf + 31, sizeof(double));
-		latitude = latitude * 180.0 / M_PI;
-		longitude = longitude * 180.0 / M_PI;
-		printf("lat= %.20lf   lon= %.20lf  | sizeof(float)=%d\n", latitude, longitude, sizeof(float));
-		*/
-		data->gps_lat = cast_8_byte_to_double(recvBuf + PI_POS_LAT) * RAD2DEG;
-		data->gps_lng = cast_8_byte_to_double(recvBuf + PI_POS_LON) * RAD2DEG;
-		data->gps_z = cast_4_byte_to_float(recvBuf + PI_POS_ALT);
-
-		/*Byte43-45. Byte46-48. Byte49-51. Velocity in units of 1 × 10^−4 m/s.*/
-		x = cast_3_byte_to_int32(recvBuf + PI_VEL_N);
-		y = cast_3_byte_to_int32(recvBuf + PI_VEL_E);
-		z = cast_3_byte_to_int32(recvBuf + PI_VEL_D);
-		if (x != INV_INT_24) data->vel_N = x * VEL2MPS;
-		if (y != INV_INT_24) data->vel_E = y * VEL2MPS;
-		if (z != INV_INT_24) data->vel_D = z * VEL2MPS;
-
-		/*Byte52-54. Heading in units of 1 × 10−6 radians. Range ±pi.*/
-		/*Byte55-57. Pitch in units of 1 × 10−6 radians. Range ±pi/2.*/
-		/*Byte58-60. Roll in units of 1 × 10−6 radians. Range ±pi.*/
-		x = cast_3_byte_to_int32(recvBuf + PI_ORIEN_H);
-		y = cast_3_byte_to_int32(recvBuf + PI_ORIEN_P);
-		z = cast_3_byte_to_int32(recvBuf + PI_ORIEN_R);
-		if (x != INV_INT_24) data->ins_heading_angle = x * (ANG2RAD * RAD2DEG);
-		if (y != INV_INT_24) data->ins_pitch_angle = y * (ANG2RAD * RAD2DEG);
-		if (z != INV_INT_24) data->ins_roll_angle = z * (ANG2RAD * RAD2DEG);
-#ifdef VV7_2
-        data->ins_heading_angle = data->ins_heading_angle+1.9;
-#endif
-#ifdef VV7_3
-        data->ins_heading_angle = data->ins_heading_angle-1.0;
-#endif
-        if(data->ins_heading_angle>360)data->ins_heading_angle = data->ins_heading_angle-360;
-		if (data->ins_heading_angle < 0.0)
-            data->ins_heading_angle += 360.0;
-
-		if (recvBuf[PI_CHANNEL_INDEX] == PI_CHANNEL_INDEX) {
-			data->rtk_status = recvBuf[PI_RTK_STATUS];
-			data->gps_satelites_num = recvBuf[PI_SAT_NUM];
-		}
-
-        ServiceCarStatus.mRTKStatus = data->rtk_status;
-
-		if (data->ins_status != 4) {
-			data->valid = IMU_STATUS_ERR;
-		} else if (data->rtk_status != 3 && data->rtk_status != 5 && data->rtk_status != 6) {
-			data->valid = RTK_STATUS_ERR;
-		} else {
-			data->valid = RTK_IMU_OK;
-		}
-
-        GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-		
-        ////从can中读取当前车速
-        //data->speed = ServiceCarStatus.speed;
-        if(data->ins_status != 0x0B)data->speed = sqrt(data->vel_N*data->vel_N + data->vel_E * data->vel_E)*3.6;
-        else continue;
-
-        if(data->gps_lat<0||data->gps_lat>90){
-            continue;
-        }
-
-        ServiceCarStatus.speed = data->speed;
- //       qDebug("speed x is %g",ServiceCarStatus.speed);
-		if(data->ins_status == 4)
-            signal_gps_data->operator()(data);	//传输数据
-		ServiceCarStatus.location->gps_lat = data->gps_lat;
-		ServiceCarStatus.location->gps_lng = data->gps_lng;
-		ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
-		ServiceCarStatus.location->gps_x = data->gps_x;
-        ServiceCarStatus.location->gps_y = data->gps_y;
-        ServiceCarStatus.location->rtk_status = data->rtk_status;
-        ServiceCarStatus.location->ins_status = data->ins_status;
- //       std::cout<<"lat = "<<ServiceCarStatus.location->gps_lat<<std::endl;
-    }
-    //close(sockfd);
-    udpsocket->close();
-}
-
-static double cast_8_byte_to_double(const uint8_t *b)
-{
-	union { double x; uint8_t c[8]; } u;
-	u.c[0] = b[0];
-	u.c[1] = b[1];
-	u.c[2] = b[2];
-	u.c[3] = b[3];
-	u.c[4] = b[4];
-	u.c[5] = b[5];
-	u.c[6] = b[6];
-	u.c[7] = b[7];
-	return u.x;
-}
-
-static float cast_4_byte_to_float(const uint8_t *b)
-{
-	union { float x; uint8_t c[4]; } u;
-	u.c[0] = b[0];
-	u.c[1] = b[1];
-	u.c[2] = b[2];
-	u.c[3] = b[3];
-	return u.x;
-}
-
-static int32_t cast_3_byte_to_int32(const uint8_t *b)
-{
-	union { int32_t x; uint8_t c[4]; } u;
-	u.c[1] = b[0];
-	u.c[2] = b[1];
-	u.c[3] = b[2];
-	return u.x >> 8;
-}
-
-
-void iv::perception::GPSSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
-{
-    int i = strlen(str), j = 0, counter = 0;
-    char c[2];
-    unsigned int bytes[2];
-
-    for (j = 0; j < i; j += 2)
-    {
-        if(0 == j % 2)
-        {
-            c[0] = str[j];
-            c[1] = str[j + 1];
-            sscanf(c, "%02x" , &bytes[0]);
-            UnChar[counter] = bytes[0];
-            counter++;
-        }
-    }
-}
-
-void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
-{
-    iv::GPSData data(new iv::GPS_INS);
-    data->ins_status = xgpsimu.ins_state();
-    data->rtk_status = xgpsimu.rtk_state();
-
-    data->accel_x = xgpsimu.acce_x();
-    data->accel_y = xgpsimu.acce_y();
-    data->accel_z = xgpsimu.acce_z();
-    data->ang_rate_x = xgpsimu.gyro_x();
-    data->ang_rate_y = xgpsimu.gyro_y();
-    data->ang_rate_z = xgpsimu.gyro_z();
-
-    data->gps_lat = xgpsimu.lat();
-    data->gps_lng = xgpsimu.lon();
-    data->gps_z = xgpsimu.height();
-
-    data->ins_heading_angle = xgpsimu.heading();
-    data->ins_pitch_angle = xgpsimu.pitch();
-    data->ins_roll_angle = xgpsimu.roll();
-
-    data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
-
-    GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-
-    ServiceCarStatus.mRTKStatus = data->rtk_status;
-
-    ServiceCarStatus.mfGPSAcc = xgpsimu.acc_calc();
-
-    ServiceCarStatus.speed = data->speed;
-//       qDebug("speed x is %g",ServiceCarStatus.speed);
-    if(data->ins_status == 4)
-        signal_gps_data->operator()(data);	//传输数据
-
-    ServiceCarStatus.location->gps_lat = data->gps_lat;
-    ServiceCarStatus.location->gps_lng = data->gps_lng;
-    ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
-    ServiceCarStatus.location->gps_x = data->gps_x;
-    ServiceCarStatus.location->gps_y = data->gps_y;
-    ServiceCarStatus.location->rtk_status = data->rtk_status;
-    ServiceCarStatus.location->ins_status = data->ins_status;
-
-}
+#include <perception/sensor_gps.h>
+#include <decition/gnss_coordinate_convert.h>
+#include <control/can.h>
+#include <QDebug>
+#include <QNetworkDatagram>
+
+static double   cast_8_byte_to_double(const uint8_t *b);
+static float    cast_4_byte_to_float(const uint8_t *b);
+static  int32_t cast_3_byte_to_int32(const uint8_t *b);
+
+void iv::perception::GPSSensor::processSensor()
+{
+
+    return;
+	//todo  GPS/惯导 设备接口 对接
+    /*Initialize udp server, listen on port 3000.*/
+    /*int sockfd;
+    struct sockaddr_in addr;
+    socklen_t addrlen;
+    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+    if (sockfd < 0)
+    {
+       printf("socket failed\n");
+       exit(EXIT_FAILURE);
+    }
+    addrlen = sizeof(struct sockaddr_in);
+    bzero(&addr, addrlen);
+    addr.sin_family = AF_INET;
+    addr.sin_addr.s_addr = htonl(INADDR_ANY);
+    addr.sin_port = htons(3000);
+    if (bind(sockfd, (struct sockaddr*)(&addr), addrlen) < 0)
+    {
+       printf("bind fail\n");
+       exit(EXIT_FAILURE);
+    }*/
+    QUdpSocket *udpsocket;//summer
+    udpsocket = new QUdpSocket();
+    udpsocket->bind(QHostAddress::Any, 3000);
+ //   udpsocket->bind(3000);
+ /*   if(!udpsocket->waitForConnected())
+    {
+        printf("bind fail\n");
+        exit(EXIT_FAILURE);
+    }*/
+    char *Buf = new char[100] ;//summer unsigned char - char
+    memset(Buf,0,100);
+    unsigned char recvBuf[100] = {0};
+
+	/*UDP initialization done. Next prepare for receiving data.*/
+
+	iv::GPSData data(new iv::GPS_INS);
+	int x, y, z;
+
+	//iv::CarStatus currentCarStatus;
+	//ODS("\n\n\n------>thread gps: %X\n\n\n\n", GetCurrentThreadId());
+	while (true) {
+        int rec = 0;
+        if(udpsocket->hasPendingDatagrams())
+        {
+  //          std::cout<<"have data."<<std::endl;
+            QNetworkDatagram datagram = udpsocket->receiveDatagram();
+            QByteArray ba = datagram.data();
+            if(ba.size() != NOUTPUT_PACKET_LENGTH)
+            {
+                continue;
+            }
+            else
+            {
+                rec = NOUTPUT_PACKET_LENGTH;
+                memcpy(recvBuf,ba.data(),NOUTPUT_PACKET_LENGTH);
+            }
+            datagram.clear();
+        }
+        else
+        {
+#ifdef Q_OS_LINUX
+            usleep(1000);
+#endif
+#ifdef WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+//            Sleep(5);
+#endif
+            continue;
+//            std::cout<<"running."<<std::endl;
+ //           std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+/*        if(udpsocket->waitForReadyRead())
+            rec = udpsocket->read(Buf,100);
+        //recvBuf = (unsigned char*)Buf;
+        convertStrToUnChar(Buf,recvBuf);//summer */
+        //int rec = recvfrom(sockfd, recvBuf, 100, 0, (struct sockaddr *)(&addr), &addrlen);
+		/*Check the receiving data*/
+		//There must be 72 bytes in one package.
+		if (rec != NOUTPUT_PACKET_LENGTH) {
+			printf("ERR: rec must be 72 bytes\n");
+			continue;
+		}
+
+		if (recvBuf[PI_SYNC] != NCOM_SYNC) {
+			printf("ERR: head always be 0xE7\n");
+			continue;
+		}
+
+        data->ins_status = recvBuf[PI_INS_NAV_MODE];
+        if(data->ins_status == 0x0B)continue;
+
+        //31.1150882 121.2211320 356.9
+        //31.1150844 121.2210996 356.9 VV7_3
+
+		/*  Start decoding
+		Details in ncomman.pdf
+		Byte		| 0	   | 1-20	| 21         | 22      | 23-60  | 61      | 62       | 63-70  | 71
+		Description	| Sync | BatchA | NaviStatus | ChkSum1 | BatchB | ChkSum2 | StatusCh | BatchS | ChkSum3
+		*/
+
+		//start decoding -> Batch A
+        //Time is transmitted as milliseconds into the current GPS minute. The range is 0–59, 999 ms.
+
+		/*
+		Acceleration x is the host object’s acceleration in the x-direction
+		(i.e.after the IMU to host attitude matrix has been applied).
+		It is a signed word in units of 1 × 10^(−4)  m / (s^2)
+		*/
+		x = cast_3_byte_to_int32(recvBuf + PI_ACCEL_X);
+		y = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Y);
+		z = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Z);
+		if (x != INV_INT_24) data->accel_x = x * ACC2MPS2;
+		if (y != INV_INT_24) data->accel_y = y * ACC2MPS2;
+		if (z != INV_INT_24) data->accel_z = z * ACC2MPS2;
+
+		/*
+		Angular rate x is the host object’s angular rate about its x-axis
+		(i.e. after the IMU to host attitude matrix has been applied).
+		It is a signed word in units of 1 × 10−5 radians/s
+		*/
+		x = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_X);
+		y = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Y);
+		z = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Z);
+		if (x != INV_INT_24) data->ang_rate_x = x * (RATE2RPS * RAD2DEG);
+		if (y != INV_INT_24) data->ang_rate_y = y * (RATE2RPS * RAD2DEG);
+		if (z != INV_INT_24) data->ang_rate_z = z * (RATE2RPS * RAD2DEG);
+
+		/*
+		The navigation status byte value should be 0–7, 10 or 20–22 to be valid
+		for customer use. See page 10. A value of 11 indicates the packet
+		follows NCOM structure-B and should be ignored.
+		*/
+		data->ins_status = recvBuf[PI_INS_NAV_MODE];
+		//navi_status refer to imu status 2 3 4.
+		//check this out in page 11 table 5.
+
+		/*
+		Checksum 1 allows the software to verify the integrity of bytes 1–21.
+		The sync byte is ignored. In low-latency applications the inertial
+		measurements in Batch A can be used to update a previous solution
+		without waiting for the rest of the packet to be received. Contact Oxford
+		Technical Solutions for source code to perform this function.
+        */
+        //31.1150796 121.2211047 355.8
+
+		//start decoding Batch B
+		/*
+		Position, orientation and velocity output. See Table 6, for a detailed
+		description.
+		*/
+		/*Byte23-30. The Latitude of the INS. It is a double in units of radians.*/
+		/*Byte31-38. Longitude of the INS. It is a double in units of radians.*/
+		/*Byte39-42. Altitude of the INS. It is a float in units of metres.*/
+		/*double latitude;
+		double longitude;
+		memcpy(&latitude, recvBuf + 23, sizeof(double));
+		memcpy(&longitude, recvBuf + 31, sizeof(double));
+		latitude = latitude * 180.0 / M_PI;
+		longitude = longitude * 180.0 / M_PI;
+		printf("lat= %.20lf   lon= %.20lf  | sizeof(float)=%d\n", latitude, longitude, sizeof(float));
+		*/
+		data->gps_lat = cast_8_byte_to_double(recvBuf + PI_POS_LAT) * RAD2DEG;
+		data->gps_lng = cast_8_byte_to_double(recvBuf + PI_POS_LON) * RAD2DEG;
+		data->gps_z = cast_4_byte_to_float(recvBuf + PI_POS_ALT);
+
+		/*Byte43-45. Byte46-48. Byte49-51. Velocity in units of 1 × 10^−4 m/s.*/
+		x = cast_3_byte_to_int32(recvBuf + PI_VEL_N);
+		y = cast_3_byte_to_int32(recvBuf + PI_VEL_E);
+		z = cast_3_byte_to_int32(recvBuf + PI_VEL_D);
+		if (x != INV_INT_24) data->vel_N = x * VEL2MPS;
+		if (y != INV_INT_24) data->vel_E = y * VEL2MPS;
+		if (z != INV_INT_24) data->vel_D = z * VEL2MPS;
+
+		/*Byte52-54. Heading in units of 1 × 10−6 radians. Range ±pi.*/
+		/*Byte55-57. Pitch in units of 1 × 10−6 radians. Range ±pi/2.*/
+		/*Byte58-60. Roll in units of 1 × 10−6 radians. Range ±pi.*/
+		x = cast_3_byte_to_int32(recvBuf + PI_ORIEN_H);
+		y = cast_3_byte_to_int32(recvBuf + PI_ORIEN_P);
+		z = cast_3_byte_to_int32(recvBuf + PI_ORIEN_R);
+		if (x != INV_INT_24) data->ins_heading_angle = x * (ANG2RAD * RAD2DEG);
+		if (y != INV_INT_24) data->ins_pitch_angle = y * (ANG2RAD * RAD2DEG);
+		if (z != INV_INT_24) data->ins_roll_angle = z * (ANG2RAD * RAD2DEG);
+#ifdef VV7_2
+        data->ins_heading_angle = data->ins_heading_angle+1.9;
+#endif
+#ifdef VV7_3
+        data->ins_heading_angle = data->ins_heading_angle-1.0;
+#endif
+        if(data->ins_heading_angle>360)data->ins_heading_angle = data->ins_heading_angle-360;
+		if (data->ins_heading_angle < 0.0)
+            data->ins_heading_angle += 360.0;
+
+		if (recvBuf[PI_CHANNEL_INDEX] == PI_CHANNEL_INDEX) {
+			data->rtk_status = recvBuf[PI_RTK_STATUS];
+			data->gps_satelites_num = recvBuf[PI_SAT_NUM];
+		}
+
+        ServiceCarStatus.mRTKStatus = data->rtk_status;
+
+		if (data->ins_status != 4) {
+			data->valid = IMU_STATUS_ERR;
+		} else if (data->rtk_status != 3 && data->rtk_status != 5 && data->rtk_status != 6) {
+			data->valid = RTK_STATUS_ERR;
+		} else {
+			data->valid = RTK_IMU_OK;
+		}
+
+        GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+		
+        ////从can中读取当前车速
+        //data->speed = ServiceCarStatus.speed;
+        if(data->ins_status != 0x0B)data->speed = sqrt(data->vel_N*data->vel_N + data->vel_E * data->vel_E)*3.6;
+        else continue;
+
+        if(data->gps_lat<0||data->gps_lat>90){
+            continue;
+        }
+
+        ServiceCarStatus.speed = data->speed;
+ //       qDebug("speed x is %g",ServiceCarStatus.speed);
+		if(data->ins_status == 4)
+            signal_gps_data->operator()(data);	//传输数据
+		ServiceCarStatus.location->gps_lat = data->gps_lat;
+		ServiceCarStatus.location->gps_lng = data->gps_lng;
+		ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
+		ServiceCarStatus.location->gps_x = data->gps_x;
+        ServiceCarStatus.location->gps_y = data->gps_y;
+        ServiceCarStatus.location->rtk_status = data->rtk_status;
+        ServiceCarStatus.location->ins_status = data->ins_status;
+ //       std::cout<<"lat = "<<ServiceCarStatus.location->gps_lat<<std::endl;
+    }
+    //close(sockfd);
+    udpsocket->close();
+}
+
+static double cast_8_byte_to_double(const uint8_t *b)
+{
+	union { double x; uint8_t c[8]; } u;
+	u.c[0] = b[0];
+	u.c[1] = b[1];
+	u.c[2] = b[2];
+	u.c[3] = b[3];
+	u.c[4] = b[4];
+	u.c[5] = b[5];
+	u.c[6] = b[6];
+	u.c[7] = b[7];
+	return u.x;
+}
+
+static float cast_4_byte_to_float(const uint8_t *b)
+{
+	union { float x; uint8_t c[4]; } u;
+	u.c[0] = b[0];
+	u.c[1] = b[1];
+	u.c[2] = b[2];
+	u.c[3] = b[3];
+	return u.x;
+}
+
+static int32_t cast_3_byte_to_int32(const uint8_t *b)
+{
+	union { int32_t x; uint8_t c[4]; } u;
+	u.c[1] = b[0];
+	u.c[2] = b[1];
+	u.c[3] = b[2];
+	return u.x >> 8;
+}
+
+
+void iv::perception::GPSSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
+{
+    int i = strlen(str), j = 0, counter = 0;
+    char c[2];
+    unsigned int bytes[2];
+
+    for (j = 0; j < i; j += 2)
+    {
+        if(0 == j % 2)
+        {
+            c[0] = str[j];
+            c[1] = str[j + 1];
+            sscanf(c, "%02x" , &bytes[0]);
+            UnChar[counter] = bytes[0];
+            counter++;
+        }
+    }
+}
+
+void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
+{
+    iv::GPSData data(new iv::GPS_INS);
+    data->ins_status = xgpsimu.ins_state();
+    data->rtk_status = xgpsimu.rtk_state();
+
+    data->accel_x = xgpsimu.acce_x();
+    data->accel_y = xgpsimu.acce_y();
+    data->accel_z = xgpsimu.acce_z();
+    data->ang_rate_x = xgpsimu.gyro_x();
+    data->ang_rate_y = xgpsimu.gyro_y();
+    data->ang_rate_z = xgpsimu.gyro_z();
+
+    data->gps_lat = xgpsimu.lat();
+    data->gps_lng = xgpsimu.lon();
+    data->gps_z = xgpsimu.height();
+
+    data->ins_heading_angle = xgpsimu.heading();
+    data->ins_pitch_angle = xgpsimu.pitch();
+    data->ins_roll_angle = xgpsimu.roll();
+
+    data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
+
+    GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+
+    ServiceCarStatus.mRTKStatus = data->rtk_status;
+
+    ServiceCarStatus.mfGPSAcc = xgpsimu.acc_calc();
+
+    ServiceCarStatus.speed = data->speed;
+//       qDebug("speed x is %g",ServiceCarStatus.speed);
+    if(data->ins_status == 4)
+        signal_gps_data->operator()(data);	//传输数据
+
+    ServiceCarStatus.location->gps_lat = data->gps_lat;
+    ServiceCarStatus.location->gps_lng = data->gps_lng;
+    ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
+    ServiceCarStatus.location->gps_x = data->gps_x;
+    ServiceCarStatus.location->gps_y = data->gps_y;
+    ServiceCarStatus.location->rtk_status = data->rtk_status;
+    ServiceCarStatus.location->ins_status = data->ins_status;
+
+}

src/decition/common/perception/impl_lidar.cpp

@@ -1,99 +1,99 @@
-/*
-* 在此方法中处理激光雷达的打开、处理数据、关闭等
-* 识别到障碍物时 触发signal_lidar_obstacle
-*
-*/
-#include <perception/sensor_lidar.h>
-#include <QDebug>
-
-#include "perceptionoutput.h"
-
-std::mutex mtx_nomal;
-
-
-
-void iv::perception::LiDARSensor::lidarcollect()
-{
-
-}
-
-void iv::perception::LiDARSensor::processSensor()
-{
-
-}
-
-
-void iv::perception::LiDARSensor::UpdatePer(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > lidar_per)
-{
-//    std::cout<<"update lidar"<<std::endl;
-    ServiceLidar.copyfromlidarper(lidar_per);
-//    ServiceLidar.copylidarperto(lidar_per);
-}
-
-void iv::perception::LiDARSensor::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
-{
-    mMutex.lock();
-    lidar_obs.swap(mobs);
-    mMutex.unlock();
-
-    ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
-    memset(ptr, 0, sizeof(Obs_grid[iv::grx][iv::gry]));
-    for (int i = 0; i <iv::grx; i++)//复制到指针数组
-    {
-        for (int j = 0; j <iv::gry; j++)
-        {
-//            ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
-//            ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
-            ptr[i * (iv::gry) + j].ob = 0;
-//            ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
-//            ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
-        }
-    }
-    for(int i=0;i<mobs->size();i++)
-    {
-        iv::ObstacleBasic xobs = mobs->at(i);
-        int dx,dy;
-        dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
-        dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
-        if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
-        {
-            ptr[dx*(iv::gry) +dy].high = xobs.high;
-            ptr[dx*(iv::gry) +dy].low = xobs.low;
-            ptr[dx*(iv::gry) + dy].ob = 2;
-            ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
-            ptr[dx*(iv::gry) + dy].pointcount = 5;
-        }
-
-    }
-
-//    std::cout<<"update obs."<<std::endl;
-    signal_lidar_obstacle_grid->operator()(ptr);//触发
-
-    ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-    int i;
-    int nSize = mobs->size();
-    for(i=0;i<nSize;i++)
-        _obs_grid_->push_back(mobs->at(i));
-    ServiceLidar.copyfromlidarobs(_obs_grid_);   
-
-    ServiceCarStatus.mLidarS = 10;
-}
-
-void iv::perception::LiDARSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
-{
-    int i = strlen(str), j = 0, counter = 0;
-    char c[2];
-    unsigned int bytes[2];
-
-    for (j = 0; j < i; j += 2)
-    {
-        if(0 == j % 2)
-        {
-            c[0] = str[j];
-            c[1] = str[j + 1];
-            sscanf(c, "%02x" , &bytes[0]);
-            UnChar[counter] = bytes[0];
-            counter++;
-        }
-    }
-}
+/*
+* 在此方法中处理激光雷达的打开、处理数据、关闭等
+* 识别到障碍物时 触发signal_lidar_obstacle
+*
+*/
+#include <perception/sensor_lidar.h>
+#include <QDebug>
+
+#include "perceptionoutput.h"
+
+std::mutex mtx_nomal;
+
+
+
+void iv::perception::LiDARSensor::lidarcollect()
+{
+
+}
+
+void iv::perception::LiDARSensor::processSensor()
+{
+
+}
+
+
+void iv::perception::LiDARSensor::UpdatePer(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > lidar_per)
+{
+//    std::cout<<"update lidar"<<std::endl;
+    ServiceLidar.copyfromlidarper(lidar_per);
+//    ServiceLidar.copylidarperto(lidar_per);
+}
+
+void iv::perception::LiDARSensor::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
+{
+    mMutex.lock();
+    lidar_obs.swap(mobs);
+    mMutex.unlock();
+
+    ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+    memset(ptr, 0, sizeof(Obs_grid[iv::grx][iv::gry]));
+    for (int i = 0; i <iv::grx; i++)//复制到指针数组
+    {
+        for (int j = 0; j <iv::gry; j++)
+        {
+//            ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
+//            ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
+            ptr[i * (iv::gry) + j].ob = 0;
+//            ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
+//            ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
+        }
+    }
+    for(int i=0;i<mobs->size();i++)
+    {
+        iv::ObstacleBasic xobs = mobs->at(i);
+        int dx,dy;
+        dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
+        dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
+        if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
+        {
+            ptr[dx*(iv::gry) +dy].high = xobs.high;
+            ptr[dx*(iv::gry) +dy].low = xobs.low;
+            ptr[dx*(iv::gry) + dy].ob = 2;
+            ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
+            ptr[dx*(iv::gry) + dy].pointcount = 5;
+        }
+
+    }
+
+//    std::cout<<"update obs."<<std::endl;
+    signal_lidar_obstacle_grid->operator()(ptr);//触发
+
+    ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+    int i;
+    int nSize = mobs->size();
+    for(i=0;i<nSize;i++)
+        _obs_grid_->push_back(mobs->at(i));
+    ServiceLidar.copyfromlidarobs(_obs_grid_);   
+
+    ServiceCarStatus.mLidarS = 10;
+}
+
+void iv::perception::LiDARSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
+{
+    int i = strlen(str), j = 0, counter = 0;
+    char c[2];
+    unsigned int bytes[2];
+
+    for (j = 0; j < i; j += 2)
+    {
+        if(0 == j % 2)
+        {
+            c[0] = str[j];
+            c[1] = str[j + 1];
+            sscanf(c, "%02x" , &bytes[0]);
+            UnChar[counter] = bytes[0];
+            counter++;
+        }
+    }
+}

src/decition/common/perception/impl_radar.cpp

@@ -1,32 +1,32 @@
-/*
-* 在此方法中处理毫米波雷达的打开、处理数据、关闭等
-* 识别到障碍物时 触发signal_radar_obstacle
-*
-*/
-
-#include <perception/sensor_radar.h>
-#include <control/can.h>
-void iv::perception::RadarSensor::processSensor()
-{
-
-}
-
-void iv::perception::RadarSensor::UpdateRADAR(iv::radar::radarobjectarray xarray)
-{
-
-    int i;
-    for(i=0;i<RADAR_OBJ_NUM;i++)
-    {
-        ServiceCarStatus.obs_radar[i].valid = false;
-    }
-
-    for(i=0;i<xarray.obj_size();i++)
-    {
-        ServiceCarStatus.obs_radar[i].valid = xarray.obj(i).bvalid();
-        ServiceCarStatus.obs_radar[i].nomal_x = xarray.obj(i).x();
-        ServiceCarStatus.obs_radar[i].nomal_y = xarray.obj(i).y();
-        ServiceCarStatus.obs_radar[i].speed_relative = xarray.obj(i).vel();
-        ServiceCarStatus.obs_radar[i].speed_x = xarray.obj(i).vx();
-        ServiceCarStatus.obs_radar[i].speed_y = xarray.obj(i).vy();
-    }
-}
+/*
+* 在此方法中处理毫米波雷达的打开、处理数据、关闭等
+* 识别到障碍物时 触发signal_radar_obstacle
+*
+*/
+
+#include <perception/sensor_radar.h>
+#include <control/can.h>
+void iv::perception::RadarSensor::processSensor()
+{
+
+}
+
+void iv::perception::RadarSensor::UpdateRADAR(iv::radar::radarobjectarray xarray)
+{
+
+    int i;
+    for(i=0;i<RADAR_OBJ_NUM;i++)
+    {
+        ServiceCarStatus.obs_radar[i].valid = false;
+    }
+
+    for(i=0;i<xarray.obj_size();i++)
+    {
+        ServiceCarStatus.obs_radar[i].valid = xarray.obj(i).bvalid();
+        ServiceCarStatus.obs_radar[i].nomal_x = xarray.obj(i).x();
+        ServiceCarStatus.obs_radar[i].nomal_y = xarray.obj(i).y();
+        ServiceCarStatus.obs_radar[i].speed_relative = xarray.obj(i).vel();
+        ServiceCarStatus.obs_radar[i].speed_x = xarray.obj(i).vx();
+        ServiceCarStatus.obs_radar[i].speed_y = xarray.obj(i).vy();
+    }
+}

src/decition/common/perception/perception.pri

@@ -1,90 +1,90 @@
-/**
-*传感器接口
-*/
-#ifndef _IV_PERCEPTION_SENSOR_
-#define _IV_PERCEPTION_SENSOR_
-
-#include <common/boost.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <string.h>
-#include <iostream>
-#include <sys/types.h>
-#include <common/obstacle_type.h>
-#include <QtNetwork/QTcpSocket>
-#include <QtNetwork/QUdpSocket>
-
-namespace iv {
-	namespace perception {
-		class Sensor
-		{
-		public:
-			Sensor() {};
-			~Sensor() {};
-
-
-            virtual void start() = 0;/*开始感知环境*/
-
-
-            virtual void stop() = 0;/* 停止感知*/
-
-
-            virtual bool isRunning() const = 0;/* 判断是否运行*/
-
-
-            virtual void processSensor() = 0;/* 传感器执行*/
-
-
-            template<typename T>//注册回调函数
-			boost::signals2::connection registerCallBack(const boost::function<T> &callback);
-			//template<typename T>
-			//boost::signals2::connection registerCallBack2(const boost::function<T> &callback);
-
-
-            template<typename T>//建立signal
-			boost::signals2::signal<T>* createSignal();
-			//template<typename T>
-			//boost::signals2::signal<T>* createSignal2();
-
-		protected:
-			boost::signals2::signal_base* signal_base;	//信号基类  存储传感器建立的信号
-			//boost::signals2::signal_base* signal_base2;	//信号基类  存储传感器建立的信号
-			boost::thread* thread_sensor_run_;			//传感器运行的线程
-		};
-
-
-
-
-        template<typename T> boost::signals2::connection// 注册回调函数
-		Sensor::registerCallBack(const boost::function<T> &callback) {
-			boost::signals2::signal<T>* signal = dynamic_cast<boost::signals2::signal<T>*>(signal_base);
-			boost::signals2::connection ret = signal->connect(callback);
-			return (ret);
-		}
-		//template<typename T> boost::signals2::connection
-		//	Sensor::registerCallBack2(const boost::function<T> &callback) {
-		//	boost::signals2::signal<T>* signal = dynamic_cast<boost::signals2::signal<T>*>(signal_base2);
-		//	boost::signals2::connection ret = signal->connect(callback);
-		//	return (ret);
-		//}
-
-		//建立signal
-		template<typename T>
-		boost::signals2::signal<T>* Sensor::createSignal() {
-			boost::signals2::signal<T>* signal = new boost::signals2::signal<T>();
-			signal_base = signal;
-			return signal;
-		}
-		//template<typename T>
-		//boost::signals2::signal<T>* Sensor::createSignal2() {
-		//	boost::signals2::signal<T>* signal = new boost::signals2::signal<T>();
-		//	signal_base2 = signal;
-		//	return signal;
-		//}
-
-	}
-}
-
-#endif 
+/**
+*传感器接口
+*/
+#ifndef _IV_PERCEPTION_SENSOR_
+#define _IV_PERCEPTION_SENSOR_
+
+#include <common/boost.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <iostream>
+#include <sys/types.h>
+#include <common/obstacle_type.h>
+#include <QtNetwork/QTcpSocket>
+#include <QtNetwork/QUdpSocket>
+
+namespace iv {
+	namespace perception {
+		class Sensor
+		{
+		public:
+			Sensor() {};
+			~Sensor() {};
+
+
+            virtual void start() = 0;/*开始感知环境*/
+
+
+            virtual void stop() = 0;/* 停止感知*/
+
+
+            virtual bool isRunning() const = 0;/* 判断是否运行*/
+
+
+            virtual void processSensor() = 0;/* 传感器执行*/
+
+
+            template<typename T>//注册回调函数
+			boost::signals2::connection registerCallBack(const boost::function<T> &callback);
+			//template<typename T>
+			//boost::signals2::connection registerCallBack2(const boost::function<T> &callback);
+
+
+            template<typename T>//建立signal
+			boost::signals2::signal<T>* createSignal();
+			//template<typename T>
+			//boost::signals2::signal<T>* createSignal2();
+
+		protected:
+			boost::signals2::signal_base* signal_base;	//信号基类  存储传感器建立的信号
+			//boost::signals2::signal_base* signal_base2;	//信号基类  存储传感器建立的信号
+			boost::thread* thread_sensor_run_;			//传感器运行的线程
+		};
+
+
+
+
+        template<typename T> boost::signals2::connection// 注册回调函数
+		Sensor::registerCallBack(const boost::function<T> &callback) {
+			boost::signals2::signal<T>* signal = dynamic_cast<boost::signals2::signal<T>*>(signal_base);
+			boost::signals2::connection ret = signal->connect(callback);
+			return (ret);
+		}
+		//template<typename T> boost::signals2::connection
+		//	Sensor::registerCallBack2(const boost::function<T> &callback) {
+		//	boost::signals2::signal<T>* signal = dynamic_cast<boost::signals2::signal<T>*>(signal_base2);
+		//	boost::signals2::connection ret = signal->connect(callback);
+		//	return (ret);
+		//}
+
+		//建立signal
+		template<typename T>
+		boost::signals2::signal<T>* Sensor::createSignal() {
+			boost::signals2::signal<T>* signal = new boost::signals2::signal<T>();
+			signal_base = signal;
+			return signal;
+		}
+		//template<typename T>
+		//boost::signals2::signal<T>* Sensor::createSignal2() {
+		//	boost::signals2::signal<T>* signal = new boost::signals2::signal<T>();
+		//	signal_base2 = signal;
+		//	return signal;
+		//}
+
+	}
+}
+
+#endif 

src/decition/common/perception/sensor_camera.cpp

@@ -1,23 +1,23 @@
-#include <perception/sensor_camera.h>
-
-iv::perception::CameraSensor::CameraSensor() {
-	signal_camera_obstacle = createSignal<sig_cb_camera_sensor_obstacle>();
-}
-
-void iv::perception::CameraSensor::start()
-{
-	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::CameraSensor::processSensor, this));
-	std::cout << "camera thread Run" << std::endl;
-}
-
-void iv::perception::CameraSensor::stop()
-{
-	thread_sensor_run_->interrupt();
-	thread_sensor_run_->join();
-	std::cout << "camera thread Stop" << std::endl;
-}
-
-bool iv::perception::CameraSensor::isRunning() const
-{
-	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
-}
+#include <perception/sensor_camera.h>
+
+iv::perception::CameraSensor::CameraSensor() {
+	signal_camera_obstacle = createSignal<sig_cb_camera_sensor_obstacle>();
+}
+
+void iv::perception::CameraSensor::start()
+{
+	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::CameraSensor::processSensor, this));
+	std::cout << "camera thread Run" << std::endl;
+}
+
+void iv::perception::CameraSensor::stop()
+{
+	thread_sensor_run_->interrupt();
+	thread_sensor_run_->join();
+	std::cout << "camera thread Stop" << std::endl;
+}
+
+bool iv::perception::CameraSensor::isRunning() const
+{
+	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
+}

src/decition/common/perception/sensor_camera.h

@@ -1,33 +1,33 @@
-#pragma once
-#ifndef _IV_PERCEPTION_SENSOR_CAMERA_
-#define _IV_PERCEPTION_SENSOR_CAMERA_
-
-#include <perception/sensor.h>
-namespace iv {
-	namespace perception {
-		class CameraSensor : public iv::perception::Sensor
-		{
-		public:
-			CameraSensor();
-			~CameraSensor();
-
-			typedef void
-			(sig_cb_camera_sensor_obstacle)(iv::ObsCamera);
-
-			// 通过 Sensor 继承
-			virtual void start() override;
-
-			virtual void stop() override;
-
-			virtual bool isRunning() const override;
-
-			virtual void processSensor() override;
-
-		private:
-			boost::signals2::signal<sig_cb_camera_sensor_obstacle>* signal_camera_obstacle;	//摄像头障碍物信号
-
-		};
-	}
-}
-
-#endif // !_IV_PERCEPTION_SENSOR_CAMERA_
+#pragma once
+#ifndef _IV_PERCEPTION_SENSOR_CAMERA_
+#define _IV_PERCEPTION_SENSOR_CAMERA_
+
+#include <perception/sensor.h>
+namespace iv {
+	namespace perception {
+		class CameraSensor : public iv::perception::Sensor
+		{
+		public:
+			CameraSensor();
+			~CameraSensor();
+
+			typedef void
+			(sig_cb_camera_sensor_obstacle)(iv::ObsCamera);
+
+			// 通过 Sensor 继承
+			virtual void start() override;
+
+			virtual void stop() override;
+
+			virtual bool isRunning() const override;
+
+			virtual void processSensor() override;
+
+		private:
+			boost::signals2::signal<sig_cb_camera_sensor_obstacle>* signal_camera_obstacle;	//摄像头障碍物信号
+
+		};
+	}
+}
+
+#endif // !_IV_PERCEPTION_SENSOR_CAMERA_

src/decition/common/perception/sensor_gps.cpp

@@ -1,63 +1,63 @@
-#include <perception/sensor_gps.h>
-#include "ivlog.h"
-
-extern std::string gstrmemgps;
-extern iv::Ivlog * givlog;
-
-namespace iv {
-namespace perception {
-    GPSSensor * ggpsimu;
-
-    void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-    {
-//        ggpsimu->UpdateGPSIMU(strdata,nSize);
-
-        iv::gps::gpsimu xgpsimu;
-        if(!xgpsimu.ParseFromArray(strdata,nSize))
-        {
-            std::cout<<"ListenRaw Parse error."<<std::endl;
-        }
-//        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());
-        ggpsimu->UpdateGPSIMU(xgpsimu);
-
-        static qint64 oldtime;
-        if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
-        {
-            qDebug("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
-            givlog->warn("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
-        }
-        givlog->verbose("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
-        oldtime = QDateTime::currentMSecsSinceEpoch();
-
-    }
-}
-}
-
-iv::perception::GPSSensor::GPSSensor() {
-	signal_gps_data = createSignal<sig_cb_gps_sensor_data>();
-    ggpsimu = this;
-}
-iv::perception::GPSSensor::~GPSSensor() {
-
-}
-
-void iv::perception::GPSSensor::start()
-{
-    thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::GPSSensor::processSensor, this));
-
-    mpagpsimu = iv::modulecomm::RegisterRecv(gstrmemgps.data(),iv::perception::Listengpsimu);
-    //OutputDebugString(TEXT("\nGPS thread Run\n"));
-}
-
-void iv::perception::GPSSensor::stop()
-{
-	thread_sensor_run_->interrupt();
-	thread_sensor_run_->join();
-    //OutputDebugString(TEXT("\nGPS thread Stop\n"));
-}
-
-bool iv::perception::GPSSensor::isRunning() const
-{
-	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
-}
-
+#include <perception/sensor_gps.h>
+#include "ivlog.h"
+
+extern std::string gstrmemgps;
+extern iv::Ivlog * givlog;
+
+namespace iv {
+namespace perception {
+    GPSSensor * ggpsimu;
+
+    void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+    {
+//        ggpsimu->UpdateGPSIMU(strdata,nSize);
+
+        iv::gps::gpsimu xgpsimu;
+        if(!xgpsimu.ParseFromArray(strdata,nSize))
+        {
+            std::cout<<"ListenRaw Parse error."<<std::endl;
+        }
+//        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());
+        ggpsimu->UpdateGPSIMU(xgpsimu);
+
+        static qint64 oldtime;
+        if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
+        {
+            qDebug("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+            givlog->warn("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+        }
+        givlog->verbose("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+        oldtime = QDateTime::currentMSecsSinceEpoch();
+
+    }
+}
+}
+
+iv::perception::GPSSensor::GPSSensor() {
+	signal_gps_data = createSignal<sig_cb_gps_sensor_data>();
+    ggpsimu = this;
+}
+iv::perception::GPSSensor::~GPSSensor() {
+
+}
+
+void iv::perception::GPSSensor::start()
+{
+    thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::GPSSensor::processSensor, this));
+
+    mpagpsimu = iv::modulecomm::RegisterRecv(gstrmemgps.data(),iv::perception::Listengpsimu);
+    //OutputDebugString(TEXT("\nGPS thread Run\n"));
+}
+
+void iv::perception::GPSSensor::stop()
+{
+	thread_sensor_run_->interrupt();
+	thread_sensor_run_->join();
+    //OutputDebugString(TEXT("\nGPS thread Stop\n"));
+}
+
+bool iv::perception::GPSSensor::isRunning() const
+{
+	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
+}
+

src/decition/common/perception/sensor_gps.h

@@ -1,222 +1,222 @@
-#pragma once
-#ifndef _IV_PERCEPTION_SENSOR_GPS_
-#define _IV_PERCEPTION_SENSOR_GPS_
-
-#include <perception/sensor.h>
-#include <common/gps_type.h>
-#include <common/boost.h>
-
-#include "modulecomm.h"
-#include "gpsimu.pb.h"
-
-// Definitions.
-
-// General constants.
-#define NOUTPUT_PACKET_LENGTH  (72)       //!< NCom packet length.
-#define NCOM_SYNC           (0xE7)        //!< NCom sync byte.
-#define PKT_PERIOD          (0.01)        //!< 10ms updates.
-#define TIME2SEC            (1e-3)        //!< Units of 1 ms.
-#define FINETIME2SEC        (4e-6)        //!< Units of 4 us.
-#define TIMECYCLE           (60000)       //!< Units of TIME2SEC (i.e. 60 seconds).
-#define WEEK2CYCLES         (10080)       //!< Time cycles in a week.
-#define ACC2MPS2            (1e-4)        //!< Units of 0.1 mm/s^2.
-#define RATE2RPS            (1e-5)        //!< Units of 0.01 mrad/s.
-#define VEL2MPS             (1e-4)        //!< Units of 0.1 mm/s.
-#define ANG2RAD             (1e-6)        //!< Units of 0.001 mrad.
-#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
-#define POSA2M              (1e-3)        //!< Units of 1 mm.
-#define VELA2MPS            (1e-3)        //!< Units of 1 mm/s.
-#define ANGA2RAD            (1e-5)        //!< Units of 0.01 mrad.
-#define GB2RPS              (5e-6)        //!< Units of 0.005 mrad/s.
-#define AB2MPS2             (1e-4)        //!< Units of 0.1 mm/s^2.
-#define GSFACTOR            (1e-6)        //!< Units of 1 ppm.
-#define ASFACTOR            (1e-6)        //!< Units of 1 ppm.
-#define GBA2RPS             (1e-6)        //!< Units of 0.001 mrad/s.
-#define ABA2MPS2            (1e-5)        //!< Units of 0.01 mm/s^2.
-#define GSAFACTOR           (1e-6)        //!< Units of 1 ppm.
-#define ASAFACTOR           (1e-6)        //!< Units of 1 ppm.
-#define GPSPOS2M            (1e-3)        //!< Units of 1 mm.
-#define GPSATT2RAD          (1e-4)        //!< Units of 0.1 mrad.
-#define GPSPOSA2M           (1e-4)        //!< Units of 0.1 mm.
-#define GPSATTA2RAD         (1e-5)        //!< Units of 0.01 mrad.
-#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
-#define DIFFAGE2SEC         (1e-2)        //!< Units of 0.01 s.
-#define REFPOS2M            (0.0012)      //!< Units of 1.2 mm.
-#define REFANG2RAD          (1e-4)        //!< Units of 0.1 mrad.
-#define OUTPOS2M            (1e-3)        //!< Units of 1 mm.
-#define ZVPOS2M             (1e-3)        //!< Units of 1 mm.
-#define ZVPOSA2M            (1e-4)        //!< Units of 0.1 mm.
-#define NSPOS2M             (1e-3)        //!< Units of 1 mm.
-#define NSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
-#define ALIGN2RAD           (1e-4)        //!< Units of 0.1 mrad.
-#define ALIGNA2RAD          (1e-5)        //!< Units of 0.01 mrad.
-#define SZVDELAY2S          (1.0)         //!< Units of 1.0 s.
-#define SZVPERIOD2S         (0.1)         //!< Units of 0.1 s.
-#define TOPSPEED2MPS        (0.5)         //!< Units of 0.5 m/s.
-#define NSDELAY2S           (0.1)         //!< Units of 0.1 s.
-#define NSPERIOD2S          (0.02)        //!< Units of 0.02 s.
-#define NSACCEL2MPS2        (0.04)        //!< Units of 0.04 m/s^2.
-#define NSSPEED2MPS         (0.1)         //!< Units of 0.1 m/s.
-#define NSRADIUS2M          (0.5)         //!< Units of 0.5 m.
-#define INITSPEED2MPS       (0.1)         //!< Units of 0.1 m/s.
-#define HLDELAY2S           (1.0)         //!< Units of 1.0 s.
-#define HLPERIOD2S          (0.1)         //!< Units of 0.1 s.
-#define STATDELAY2S         (1.0)         //!< Units of 1.0 s.
-#define STATSPEED2MPS       (0.01)        //!< Units of 1.0 cm/s.
-#define WSPOS2M             (1e-3)        //!< Units of 1 mm.
-#define WSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
-#define WSSF2PPM            (0.1)         //!< Units of 0.1 pulse per metre (ppm).
-#define WSSFA2PC            (0.002)       //!< Units of 0.002% of scale factor.
-#define WSDELAY2S           (0.1)         //!< Units of 0.1 s.
-#define WSNOISE2CNT         (0.1)         //!< Units of 0.1 count for wheel speed noise.
-#define UNDUL2M             (0.005)       //!< Units of 5 mm.
-#define DOPFACTOR           (0.1)         //!< Resolution of 0.1.
-#define OMNISTAR_MIN_FREQ   (1.52e9)      //!< (Hz) i.e. 1520.0 MHz.
-#define OMNIFREQ2HZ         (1000.0)      //!< Resolution of 1 kHz.
-#define SNR2DB              (0.2)         //!< Resolution of 0.2 dB.
-#define LTIME2SEC           (1.0)         //!< Resolution of 1.0 s.
-#define TEMPK_OFFSET        (203.15)      //!< Temperature offset in degrees K.
-#define ABSZERO_TEMPC       (-273.15)     //!< Absolute zero (i.e. 0 deg K) in deg C.
-
-// For more accurate and complete local coordinates
-#define FINEANG2RAD (1.74532925199433e-9) //!< Units of 0.1 udeg.
-#define ALT2M               (1e-3)        //!< Units of 1 mm.
-
-// For GPS supply voltage
-#define SUPPLYV2V           (0.1)         //!< Units of 0.1 V.
-
-// Mathematical constant definitions
-#ifndef M_PI
-#define M_PI (3.1415926535897932384626433832795)  //!< Pi.
-#endif
-#define DEG2RAD             (M_PI/180.0)  //!< Convert degrees to radians.
-#define RAD2DEG             (180.0/M_PI)  //!< Convert radians to degrees.
-#define POS_INT_24          (8388607)     //!< Maximum value of a two's complement 24 bit integer.
-#define NEG_INT_24          (-8388607)    //!< Minimum value of a two's complement 24 bit integer.
-#define INV_INT_24          (-8388608)    //!< Represents an invalid two's complement 24 bit integer.
-
-#define NCOM_COUNT_TOO_OLD  (150)         //!< Cycle counter for data too old.
-#define NCOM_STDCNT_MAX     (0xFF)        //!< Definition for the RTBNS accuracy counter.
-#define MIN_HORZ_SPEED      (0.07)        //!< 0.07 m/s hold distance.
-#define MIN_VERT_SPEED      (0.07)        //!< 0.07 m/s hold distance.
-#define SPEED_HOLD_FACTOR   (2.0)         //!< Hold distance when speed within 2 sigma of 0.
-#define MINUTES_IN_WEEK     (10080)       //!< Number of minutes in a week.
-
-// OmniStar status definitions
-#define NCOM_OMNI_STATUS_UNKNOWN      (0xFF)
-#define NCOM_OMNI_STATUS_VBSEXPIRED   (0x01)
-#define NCOM_OMNI_STATUS_VBSREGION    (0x02)
-#define NCOM_OMNI_STATUS_VBSNOBASE    (0x04)
-#define NCOM_OMNI_STATUS_HPEXPIRED    (0x08)
-#define NCOM_OMNI_STATUS_HPREGION     (0x10)
-#define NCOM_OMNI_STATUS_HPNOBASE     (0x20)
-#define NCOM_OMNI_STATUS_HPNOCONVERGE (0x40)
-#define NCOM_OMNI_STATUS_HPKEYINVALID (0x80)
-
-// GPS hardware status definitions
-#define NCOM_GPS_ANT_STATUS_BITMASK   (0x03)
-#define NCOM_GPS_ANT_STATUS_DONTKNOW  (0x03)
-#define NCOM_GPS_ANT_STATUS_BITSHIFT  (0)
-#define NCOM_GPS_ANT_POWER_BITMASK    (0x0C)
-#define NCOM_GPS_ANT_POWER_DONTKNOW   (0x0C)
-#define NCOM_GPS_ANT_POWER_BITSHIFT   (2)
-
-// GPS feature set 1 definitions
-#define NCOM_GPS_FEATURE_PSRDIFF      (0x01)
-#define NCOM_GPS_FEATURE_SBAS         (0x02)
-#define NCOM_GPS_FEATURE_OMNIVBS      (0x08)
-#define NCOM_GPS_FEATURE_OMNIHP       (0x10)
-#define NCOM_GPS_FEATURE_L1DIFF       (0x20)
-#define NCOM_GPS_FEATURE_L1L2DIFF     (0x40)
-
-// GPS feature set 2 definitions
-#define NCOM_GPS_FEATURE_GLONASS      (0x01)
-#define NCOM_GPS_FEATURE_GALILEO      (0x02)
-#define NCOM_GPS_FEATURE_RAWRNG       (0x04)
-#define NCOM_GPS_FEATURE_RAWDOP       (0x08)
-#define NCOM_GPS_FEATURE_RAWL1        (0x10)
-#define NCOM_GPS_FEATURE_RAWL2        (0x20)
-#define NCOM_GPS_FEATURE_RAWL5        (0x40)
-
-// GPS feature valid definition
-#define NCOM_GPS_FEATURE_VALID        (0x80)
-
-// The start of GPS time in a time_t style. In this version it is a constant, but this constant assumes that
-// the local machine uses 00:00:00 01/01/1970 as its Epoch time. If your machine is different then you need to
-// convert 00:00:00 06/01/1980 in to the local machine's time_t time.
-#define GPS_TIME_START_TIME_T         (315964800)
-
-// Second order filter class
-#define INPUT_JITTER_TOLERANCE     (0.01)  // i.e. 1%
-
-
-/*index*/
-#define		PI_SYNC             0
-#define		PI_TIME             1
-#define		PI_ACCEL_X          3
-#define		PI_ACCEL_Y          6
-#define		PI_ACCEL_Z          9
-#define		PI_ANG_RATE_X      12
-#define		PI_ANG_RATE_Y      15
-#define		PI_ANG_RATE_Z      18
-#define		PI_INS_NAV_MODE    21
-#define		PI_CHECKSUM_1      22
-#define		PI_POS_LAT         23
-#define		PI_POS_LON         31
-#define		PI_POS_ALT         39
-#define		PI_VEL_N           43
-#define		PI_VEL_E           46
-#define		PI_VEL_D           49
-#define		PI_ORIEN_H         52
-#define		PI_ORIEN_P         55
-#define		PI_ORIEN_R         58
-#define		PI_CHECKSUM_2      61
-#define		PI_CHANNEL_INDEX   62
-#define		PI_CHANNEL_STATUS  63
-#define		PI_SAT_NUM		   67
-#define		PI_RTK_STATUS	   68
-#define		PI_CHECKSUM_3      71
-
-/*RTK IMU status check*/
-#define		RTK_IMU_OK		0
-#define		IMU_STATUS_ERR	1
-#define		RTK_STATUS_ERR	2
-#define		UNKNOWN			0xFF
-
-namespace iv {
-	namespace perception {
-		class GPSSensor : public iv::perception::Sensor
-		{
-		public:
-			GPSSensor();
-			~GPSSensor();
-
-			// 通过 Sensor 继承
-			virtual void start() override;
-
-			virtual void stop() override;
-
-			virtual bool isRunning() const override;
-
-			virtual void processSensor() override;
-            void convertStrToUnChar(char* str, unsigned char* UnChar);
-
-			/* gps 惯导 数据*/
-			typedef void
-			(sig_cb_gps_sensor_data)(iv::GPSData);
-
-		private:
-			boost::signals2::signal<sig_cb_gps_sensor_data>* signal_gps_data;	//GPS 惯导数据信号
-
-        private:
-            void * mpagpsimu;
-
-        public:
-            void UpdateGPSIMU(iv::gps::gpsimu xgpsimu);
-		};
-
-
-	}
-}
-
-#endif // !_IV_PERCEPTION_SENSOR_GPS_
+#pragma once
+#ifndef _IV_PERCEPTION_SENSOR_GPS_
+#define _IV_PERCEPTION_SENSOR_GPS_
+
+#include <perception/sensor.h>
+#include <common/gps_type.h>
+#include <common/boost.h>
+
+#include "modulecomm.h"
+#include "gpsimu.pb.h"
+
+// Definitions.
+
+// General constants.
+#define NOUTPUT_PACKET_LENGTH  (72)       //!< NCom packet length.
+#define NCOM_SYNC           (0xE7)        //!< NCom sync byte.
+#define PKT_PERIOD          (0.01)        //!< 10ms updates.
+#define TIME2SEC            (1e-3)        //!< Units of 1 ms.
+#define FINETIME2SEC        (4e-6)        //!< Units of 4 us.
+#define TIMECYCLE           (60000)       //!< Units of TIME2SEC (i.e. 60 seconds).
+#define WEEK2CYCLES         (10080)       //!< Time cycles in a week.
+#define ACC2MPS2            (1e-4)        //!< Units of 0.1 mm/s^2.
+#define RATE2RPS            (1e-5)        //!< Units of 0.01 mrad/s.
+#define VEL2MPS             (1e-4)        //!< Units of 0.1 mm/s.
+#define ANG2RAD             (1e-6)        //!< Units of 0.001 mrad.
+#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
+#define POSA2M              (1e-3)        //!< Units of 1 mm.
+#define VELA2MPS            (1e-3)        //!< Units of 1 mm/s.
+#define ANGA2RAD            (1e-5)        //!< Units of 0.01 mrad.
+#define GB2RPS              (5e-6)        //!< Units of 0.005 mrad/s.
+#define AB2MPS2             (1e-4)        //!< Units of 0.1 mm/s^2.
+#define GSFACTOR            (1e-6)        //!< Units of 1 ppm.
+#define ASFACTOR            (1e-6)        //!< Units of 1 ppm.
+#define GBA2RPS             (1e-6)        //!< Units of 0.001 mrad/s.
+#define ABA2MPS2            (1e-5)        //!< Units of 0.01 mm/s^2.
+#define GSAFACTOR           (1e-6)        //!< Units of 1 ppm.
+#define ASAFACTOR           (1e-6)        //!< Units of 1 ppm.
+#define GPSPOS2M            (1e-3)        //!< Units of 1 mm.
+#define GPSATT2RAD          (1e-4)        //!< Units of 0.1 mrad.
+#define GPSPOSA2M           (1e-4)        //!< Units of 0.1 mm.
+#define GPSATTA2RAD         (1e-5)        //!< Units of 0.01 mrad.
+#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
+#define DIFFAGE2SEC         (1e-2)        //!< Units of 0.01 s.
+#define REFPOS2M            (0.0012)      //!< Units of 1.2 mm.
+#define REFANG2RAD          (1e-4)        //!< Units of 0.1 mrad.
+#define OUTPOS2M            (1e-3)        //!< Units of 1 mm.
+#define ZVPOS2M             (1e-3)        //!< Units of 1 mm.
+#define ZVPOSA2M            (1e-4)        //!< Units of 0.1 mm.
+#define NSPOS2M             (1e-3)        //!< Units of 1 mm.
+#define NSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
+#define ALIGN2RAD           (1e-4)        //!< Units of 0.1 mrad.
+#define ALIGNA2RAD          (1e-5)        //!< Units of 0.01 mrad.
+#define SZVDELAY2S          (1.0)         //!< Units of 1.0 s.
+#define SZVPERIOD2S         (0.1)         //!< Units of 0.1 s.
+#define TOPSPEED2MPS        (0.5)         //!< Units of 0.5 m/s.
+#define NSDELAY2S           (0.1)         //!< Units of 0.1 s.
+#define NSPERIOD2S          (0.02)        //!< Units of 0.02 s.
+#define NSACCEL2MPS2        (0.04)        //!< Units of 0.04 m/s^2.
+#define NSSPEED2MPS         (0.1)         //!< Units of 0.1 m/s.
+#define NSRADIUS2M          (0.5)         //!< Units of 0.5 m.
+#define INITSPEED2MPS       (0.1)         //!< Units of 0.1 m/s.
+#define HLDELAY2S           (1.0)         //!< Units of 1.0 s.
+#define HLPERIOD2S          (0.1)         //!< Units of 0.1 s.
+#define STATDELAY2S         (1.0)         //!< Units of 1.0 s.
+#define STATSPEED2MPS       (0.01)        //!< Units of 1.0 cm/s.
+#define WSPOS2M             (1e-3)        //!< Units of 1 mm.
+#define WSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
+#define WSSF2PPM            (0.1)         //!< Units of 0.1 pulse per metre (ppm).
+#define WSSFA2PC            (0.002)       //!< Units of 0.002% of scale factor.
+#define WSDELAY2S           (0.1)         //!< Units of 0.1 s.
+#define WSNOISE2CNT         (0.1)         //!< Units of 0.1 count for wheel speed noise.
+#define UNDUL2M             (0.005)       //!< Units of 5 mm.
+#define DOPFACTOR           (0.1)         //!< Resolution of 0.1.
+#define OMNISTAR_MIN_FREQ   (1.52e9)      //!< (Hz) i.e. 1520.0 MHz.
+#define OMNIFREQ2HZ         (1000.0)      //!< Resolution of 1 kHz.
+#define SNR2DB              (0.2)         //!< Resolution of 0.2 dB.
+#define LTIME2SEC           (1.0)         //!< Resolution of 1.0 s.
+#define TEMPK_OFFSET        (203.15)      //!< Temperature offset in degrees K.
+#define ABSZERO_TEMPC       (-273.15)     //!< Absolute zero (i.e. 0 deg K) in deg C.
+
+// For more accurate and complete local coordinates
+#define FINEANG2RAD (1.74532925199433e-9) //!< Units of 0.1 udeg.
+#define ALT2M               (1e-3)        //!< Units of 1 mm.
+
+// For GPS supply voltage
+#define SUPPLYV2V           (0.1)         //!< Units of 0.1 V.
+
+// Mathematical constant definitions
+#ifndef M_PI
+#define M_PI (3.1415926535897932384626433832795)  //!< Pi.
+#endif
+#define DEG2RAD             (M_PI/180.0)  //!< Convert degrees to radians.
+#define RAD2DEG             (180.0/M_PI)  //!< Convert radians to degrees.
+#define POS_INT_24          (8388607)     //!< Maximum value of a two's complement 24 bit integer.
+#define NEG_INT_24          (-8388607)    //!< Minimum value of a two's complement 24 bit integer.
+#define INV_INT_24          (-8388608)    //!< Represents an invalid two's complement 24 bit integer.
+
+#define NCOM_COUNT_TOO_OLD  (150)         //!< Cycle counter for data too old.
+#define NCOM_STDCNT_MAX     (0xFF)        //!< Definition for the RTBNS accuracy counter.
+#define MIN_HORZ_SPEED      (0.07)        //!< 0.07 m/s hold distance.
+#define MIN_VERT_SPEED      (0.07)        //!< 0.07 m/s hold distance.
+#define SPEED_HOLD_FACTOR   (2.0)         //!< Hold distance when speed within 2 sigma of 0.
+#define MINUTES_IN_WEEK     (10080)       //!< Number of minutes in a week.
+
+// OmniStar status definitions
+#define NCOM_OMNI_STATUS_UNKNOWN      (0xFF)
+#define NCOM_OMNI_STATUS_VBSEXPIRED   (0x01)
+#define NCOM_OMNI_STATUS_VBSREGION    (0x02)
+#define NCOM_OMNI_STATUS_VBSNOBASE    (0x04)
+#define NCOM_OMNI_STATUS_HPEXPIRED    (0x08)
+#define NCOM_OMNI_STATUS_HPREGION     (0x10)
+#define NCOM_OMNI_STATUS_HPNOBASE     (0x20)
+#define NCOM_OMNI_STATUS_HPNOCONVERGE (0x40)
+#define NCOM_OMNI_STATUS_HPKEYINVALID (0x80)
+
+// GPS hardware status definitions
+#define NCOM_GPS_ANT_STATUS_BITMASK   (0x03)
+#define NCOM_GPS_ANT_STATUS_DONTKNOW  (0x03)
+#define NCOM_GPS_ANT_STATUS_BITSHIFT  (0)
+#define NCOM_GPS_ANT_POWER_BITMASK    (0x0C)
+#define NCOM_GPS_ANT_POWER_DONTKNOW   (0x0C)
+#define NCOM_GPS_ANT_POWER_BITSHIFT   (2)
+
+// GPS feature set 1 definitions
+#define NCOM_GPS_FEATURE_PSRDIFF      (0x01)
+#define NCOM_GPS_FEATURE_SBAS         (0x02)
+#define NCOM_GPS_FEATURE_OMNIVBS      (0x08)
+#define NCOM_GPS_FEATURE_OMNIHP       (0x10)
+#define NCOM_GPS_FEATURE_L1DIFF       (0x20)
+#define NCOM_GPS_FEATURE_L1L2DIFF     (0x40)
+
+// GPS feature set 2 definitions
+#define NCOM_GPS_FEATURE_GLONASS      (0x01)
+#define NCOM_GPS_FEATURE_GALILEO      (0x02)
+#define NCOM_GPS_FEATURE_RAWRNG       (0x04)
+#define NCOM_GPS_FEATURE_RAWDOP       (0x08)
+#define NCOM_GPS_FEATURE_RAWL1        (0x10)
+#define NCOM_GPS_FEATURE_RAWL2        (0x20)
+#define NCOM_GPS_FEATURE_RAWL5        (0x40)
+
+// GPS feature valid definition
+#define NCOM_GPS_FEATURE_VALID        (0x80)
+
+// The start of GPS time in a time_t style. In this version it is a constant, but this constant assumes that
+// the local machine uses 00:00:00 01/01/1970 as its Epoch time. If your machine is different then you need to
+// convert 00:00:00 06/01/1980 in to the local machine's time_t time.
+#define GPS_TIME_START_TIME_T         (315964800)
+
+// Second order filter class
+#define INPUT_JITTER_TOLERANCE     (0.01)  // i.e. 1%
+
+
+/*index*/
+#define		PI_SYNC             0
+#define		PI_TIME             1
+#define		PI_ACCEL_X          3
+#define		PI_ACCEL_Y          6
+#define		PI_ACCEL_Z          9
+#define		PI_ANG_RATE_X      12
+#define		PI_ANG_RATE_Y      15
+#define		PI_ANG_RATE_Z      18
+#define		PI_INS_NAV_MODE    21
+#define		PI_CHECKSUM_1      22
+#define		PI_POS_LAT         23
+#define		PI_POS_LON         31
+#define		PI_POS_ALT         39
+#define		PI_VEL_N           43
+#define		PI_VEL_E           46
+#define		PI_VEL_D           49
+#define		PI_ORIEN_H         52
+#define		PI_ORIEN_P         55
+#define		PI_ORIEN_R         58
+#define		PI_CHECKSUM_2      61
+#define		PI_CHANNEL_INDEX   62
+#define		PI_CHANNEL_STATUS  63
+#define		PI_SAT_NUM		   67
+#define		PI_RTK_STATUS	   68
+#define		PI_CHECKSUM_3      71
+
+/*RTK IMU status check*/
+#define		RTK_IMU_OK		0
+#define		IMU_STATUS_ERR	1
+#define		RTK_STATUS_ERR	2
+#define		UNKNOWN			0xFF
+
+namespace iv {
+	namespace perception {
+		class GPSSensor : public iv::perception::Sensor
+		{
+		public:
+			GPSSensor();
+			~GPSSensor();
+
+			// 通过 Sensor 继承
+			virtual void start() override;
+
+			virtual void stop() override;
+
+			virtual bool isRunning() const override;
+
+			virtual void processSensor() override;
+            void convertStrToUnChar(char* str, unsigned char* UnChar);
+
+			/* gps 惯导 数据*/
+			typedef void
+			(sig_cb_gps_sensor_data)(iv::GPSData);
+
+		private:
+			boost::signals2::signal<sig_cb_gps_sensor_data>* signal_gps_data;	//GPS 惯导数据信号
+
+        private:
+            void * mpagpsimu;
+
+        public:
+            void UpdateGPSIMU(iv::gps::gpsimu xgpsimu);
+		};
+
+
+	}
+}
+
+#endif // !_IV_PERCEPTION_SENSOR_GPS_

src/decition/common/perception/sensor_lidar.cpp

@@ -1,101 +1,101 @@
-#include <perception/sensor_lidar.h>
-
-namespace iv {
-namespace perception {
-    LiDARSensor * gipl;
-}
-}
-
-iv::perception::LiDARSensor::LiDARSensor() {
-    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
-    mobs = lidar_obs;
-    mbRun = false;
-    gipl = this;
-	signal_lidar_obstacle_grid = createSignal<sig_cb_lidar_sensor_obstacle_grid>();
-    JiGuangLeiDa = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-}
-iv::perception::LiDARSensor::~LiDARSensor() {
-
-}
-
-void ListenPer(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-//    qDebug("size is %d",nSize);
-
-    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
-
-
-    iv::Perception::PerceptionOutput * pdata = (iv::Perception::PerceptionOutput *)strdata;
-    int nCount = nSize/sizeof(iv::Perception::PerceptionOutput);
-    int i;
-    for(i=0;i<nCount;i++)
-    {
-        iv::Perception::PerceptionOutput temp;
-        memcpy(&temp,pdata,sizeof(iv::Perception::PerceptionOutput));
-        lidar_per->push_back(temp);
-        pdata++;
-    }
-    iv::perception::gipl->UpdatePer(lidar_per);
-
-  //  gw->UpdateOBS(lidar_obs);
-}
-
-void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-//    qDebug("size is %d",nSize);
-
-    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
-
-
-    iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
-    int nCount = nSize/sizeof(iv::ObstacleBasic);
-    int i;
-    for(i=0;i<nCount;i++)
-    {
-        iv::ObstacleBasic temp;
-        memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
-        lidar_obs->push_back(temp);
-        pdata++;
-    }
-    iv::perception::gipl->UpdateOBS(lidar_obs);
-
-  //  gw->UpdateOBS(lidar_obs);
-}
-
-
-void iv::perception::LiDARSensor::start() {
-
-
-    iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
-//    mpa = new adcmemshare("lidar_obs",20000000,3);
-//    mpa->listenmsg(ListenOBS);
-
-    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
-//    mpb = new adcmemshare("lidar_per",20000000,3);
-//    mpb->listenmsg(ListenPer);
-    mbRun = true;
-    return;
-
-//	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::LiDARSensor::processSensor, this));
-//	thread_sensor_run_collect = new boost::thread(boost::bind(&iv::perception::LiDARSensor::lidarcollect, this));
-//	std::cout << "LiDAR thread Run" << std::endl;
-
-}
-
-void iv::perception::LiDARSensor::stop() {
-    delete mpa;
-    mbRun = false;
-    return;
-	thread_sensor_run_->interrupt();
-	thread_sensor_run_->join();
-	thread_sensor_run_collect->interrupt();
-	thread_sensor_run_collect->join();
-	std::cout << "LiDAR thread Stop" << std::endl;
-}
-
-bool iv::perception::LiDARSensor::isRunning() const {
-    return mbRun;
-	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)) && thread_sensor_run_collect != NULL && !thread_sensor_run_collect->timed_join(boost::posix_time::milliseconds(10)));
-}
+#include <perception/sensor_lidar.h>
+
+namespace iv {
+namespace perception {
+    LiDARSensor * gipl;
+}
+}
+
+iv::perception::LiDARSensor::LiDARSensor() {
+    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
+    mobs = lidar_obs;
+    mbRun = false;
+    gipl = this;
+	signal_lidar_obstacle_grid = createSignal<sig_cb_lidar_sensor_obstacle_grid>();
+    JiGuangLeiDa = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+}
+iv::perception::LiDARSensor::~LiDARSensor() {
+
+}
+
+void ListenPer(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+//    qDebug("size is %d",nSize);
+
+    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
+
+
+    iv::Perception::PerceptionOutput * pdata = (iv::Perception::PerceptionOutput *)strdata;
+    int nCount = nSize/sizeof(iv::Perception::PerceptionOutput);
+    int i;
+    for(i=0;i<nCount;i++)
+    {
+        iv::Perception::PerceptionOutput temp;
+        memcpy(&temp,pdata,sizeof(iv::Perception::PerceptionOutput));
+        lidar_per->push_back(temp);
+        pdata++;
+    }
+    iv::perception::gipl->UpdatePer(lidar_per);
+
+  //  gw->UpdateOBS(lidar_obs);
+}
+
+void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+//    qDebug("size is %d",nSize);
+
+    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
+
+
+    iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
+    int nCount = nSize/sizeof(iv::ObstacleBasic);
+    int i;
+    for(i=0;i<nCount;i++)
+    {
+        iv::ObstacleBasic temp;
+        memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
+        lidar_obs->push_back(temp);
+        pdata++;
+    }
+    iv::perception::gipl->UpdateOBS(lidar_obs);
+
+  //  gw->UpdateOBS(lidar_obs);
+}
+
+
+void iv::perception::LiDARSensor::start() {
+
+
+    iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
+//    mpa = new adcmemshare("lidar_obs",20000000,3);
+//    mpa->listenmsg(ListenOBS);
+
+    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
+//    mpb = new adcmemshare("lidar_per",20000000,3);
+//    mpb->listenmsg(ListenPer);
+    mbRun = true;
+    return;
+
+//	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::LiDARSensor::processSensor, this));
+//	thread_sensor_run_collect = new boost::thread(boost::bind(&iv::perception::LiDARSensor::lidarcollect, this));
+//	std::cout << "LiDAR thread Run" << std::endl;
+
+}
+
+void iv::perception::LiDARSensor::stop() {
+    delete mpa;
+    mbRun = false;
+    return;
+	thread_sensor_run_->interrupt();
+	thread_sensor_run_->join();
+	thread_sensor_run_collect->interrupt();
+	thread_sensor_run_collect->join();
+	std::cout << "LiDAR thread Stop" << std::endl;
+}
+
+bool iv::perception::LiDARSensor::isRunning() const {
+    return mbRun;
+	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)) && thread_sensor_run_collect != NULL && !thread_sensor_run_collect->timed_join(boost::posix_time::milliseconds(10)));
+}

src/decition/common/perception/sensor_lidar.h

@@ -1,81 +1,81 @@
-#pragma once
-/**
-* 激光雷达
-*/
-#ifndef _IV_PERCEPTION_LIDARSENSOR_
-#define	_IV_PERCEPTION_LIDARSENSOR_
-
-#define Lidar_Pi 3.1415926535897932384626433832795
-#define Lidar32 (unsigned long)3405883584//192.168.1.203
-#define Lidar_roll_ang -3.15*Lidar_Pi/180.0
-
-#include <common/boost.h>
-#include <common/lidar.h>
-#include <perception/sensor.h>
-#include <mutex>
-#include <stdlib.h>
-#include <stdio.h>
-#include <cmath>
-#include <QMutex>
-
-#include "modulecomm.h"
-
-namespace iv {
-	//CRITICAL_SECTION cs;
-	namespace perception {
-		class LiDARSensor : public iv::perception::Sensor
-		{
-		public:
-			LiDARSensor() ;
-			~LiDARSensor();
-
-			virtual void start() override;
-
-			virtual void stop() override;
-
-			virtual bool isRunning() const override;
-
-			virtual void processSensor() override;	
-
-            void UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs);
-
-            void UpdatePer(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per);
-
-            QMutex mMutex;
-
-            std::shared_ptr<std::vector<iv::ObstacleBasic>> mobs;
-			
-            void lidarcollect();
-             void convertStrToUnChar(char* str, unsigned char* UnChar);
-
-			boost::thread* thread_sensor_run_collect;			//传感器运行的线程2
-
-			/* 激光雷达障碍物*/
-			typedef void
-			(sig_cb_lidar_sensor_obstacle_grid)(iv::LidarGridPtr);
-		
-		private:
-			boost::signals2::signal<sig_cb_lidar_sensor_obstacle_grid>* signal_lidar_obstacle_grid;	//激光雷达栅格
-
-            unsigned char recvBuf1[320][1207];
-            unsigned char recvBuf2[320][1207];
-            int buf1len;
-            int buf2len;
-            int dx;
-            int dy;
-            bool bufuse_1 = false;//true:数据处理中，不能写入  false：完成处理，可以写入
-            bool bufuse_2 = false;
-            iv::ObsLiDAR JiGuangLeiDa;
-            iv::Obs_grid obs_grid[iv::grx + 1][iv::gry + 1];
-            iv::LidarGridPtr ptr;
-
-            void * mpa;
-            void * mpb;
-            bool mbRun;
-
-		};
-
-	}
-}
-
-#endif // !_IV_PERCEPTION_LIDARSENSOR_
+#pragma once
+/**
+* 激光雷达
+*/
+#ifndef _IV_PERCEPTION_LIDARSENSOR_
+#define	_IV_PERCEPTION_LIDARSENSOR_
+
+#define Lidar_Pi 3.1415926535897932384626433832795
+#define Lidar32 (unsigned long)3405883584//192.168.1.203
+#define Lidar_roll_ang -3.15*Lidar_Pi/180.0
+
+#include <common/boost.h>
+#include <common/lidar.h>
+#include <perception/sensor.h>
+#include <mutex>
+#include <stdlib.h>
+#include <stdio.h>
+#include <cmath>
+#include <QMutex>
+
+#include "modulecomm.h"
+
+namespace iv {
+	//CRITICAL_SECTION cs;
+	namespace perception {
+		class LiDARSensor : public iv::perception::Sensor
+		{
+		public:
+			LiDARSensor() ;
+			~LiDARSensor();
+
+			virtual void start() override;
+
+			virtual void stop() override;
+
+			virtual bool isRunning() const override;
+
+			virtual void processSensor() override;	
+
+            void UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs);
+
+            void UpdatePer(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per);
+
+            QMutex mMutex;
+
+            std::shared_ptr<std::vector<iv::ObstacleBasic>> mobs;
+			
+            void lidarcollect();
+             void convertStrToUnChar(char* str, unsigned char* UnChar);
+
+			boost::thread* thread_sensor_run_collect;			//传感器运行的线程2
+
+			/* 激光雷达障碍物*/
+			typedef void
+			(sig_cb_lidar_sensor_obstacle_grid)(iv::LidarGridPtr);
+		
+		private:
+			boost::signals2::signal<sig_cb_lidar_sensor_obstacle_grid>* signal_lidar_obstacle_grid;	//激光雷达栅格
+
+            unsigned char recvBuf1[320][1207];
+            unsigned char recvBuf2[320][1207];
+            int buf1len;
+            int buf2len;
+            int dx;
+            int dy;
+            bool bufuse_1 = false;//true:数据处理中，不能写入  false：完成处理，可以写入
+            bool bufuse_2 = false;
+            iv::ObsLiDAR JiGuangLeiDa;
+            iv::Obs_grid obs_grid[iv::grx + 1][iv::gry + 1];
+            iv::LidarGridPtr ptr;
+
+            void * mpa;
+            void * mpb;
+            bool mbRun;
+
+		};
+
+	}
+}
+
+#endif // !_IV_PERCEPTION_LIDARSENSOR_

src/decition/common/perception/sensor_radar.cpp

@@ -1,55 +1,55 @@
-#include <perception/sensor_radar.h>
-
-
-
-extern std::string gstrmemradar;
-
-
-namespace iv {
-namespace perception {
-    RadarSensor * gRadar;
-
-    void ListenRadar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-    {
-        if(nSize<1)return;
-        iv::radar::radarobjectarray xobj;
-
-        if(false == xobj.ParseFromArray(strdata,nSize))
-        {
-            std::cout<<"ListenenRadar fail."<<std::endl;
-            return;
-        }
-        gRadar->UpdateRADAR(xobj);
-    }
-}
-}
-
-iv::perception::RadarSensor::RadarSensor() {
-	signal_radar_obstacle = createSignal<sig_cb_radar_sensor_obstacle>();
-
-    iv::perception::gRadar = this;
-
-    mparadar = iv::modulecomm::RegisterRecv(gstrmemradar.data(),iv::perception::ListenRadar);
-//    mparadar = new adcmemshare("radar",10*sizeof(iv::ObstacleBasic)*64,10);
-//    mparadar->listenmsg(iv::perception::ListenRadar);
-}
-
-
-void iv::perception::RadarSensor::start()
-{
-	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::RadarSensor::processSensor, this));
-	std::cout << "Radar Thread Run" << std::endl;
-}
-
-void iv::perception::RadarSensor::stop()
-{
-	thread_sensor_run_->interrupt();
-	thread_sensor_run_->join();
-	std::cout << "Radar Thread Stop" << std::endl;
-}
-
-bool iv::perception::RadarSensor::isRunning() const
-{
-	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
-}
-
+#include <perception/sensor_radar.h>
+
+
+
+extern std::string gstrmemradar;
+
+
+namespace iv {
+namespace perception {
+    RadarSensor * gRadar;
+
+    void ListenRadar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+    {
+        if(nSize<1)return;
+        iv::radar::radarobjectarray xobj;
+
+        if(false == xobj.ParseFromArray(strdata,nSize))
+        {
+            std::cout<<"ListenenRadar fail."<<std::endl;
+            return;
+        }
+        gRadar->UpdateRADAR(xobj);
+    }
+}
+}
+
+iv::perception::RadarSensor::RadarSensor() {
+	signal_radar_obstacle = createSignal<sig_cb_radar_sensor_obstacle>();
+
+    iv::perception::gRadar = this;
+
+    mparadar = iv::modulecomm::RegisterRecv(gstrmemradar.data(),iv::perception::ListenRadar);
+//    mparadar = new adcmemshare("radar",10*sizeof(iv::ObstacleBasic)*64,10);
+//    mparadar->listenmsg(iv::perception::ListenRadar);
+}
+
+
+void iv::perception::RadarSensor::start()
+{
+	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::RadarSensor::processSensor, this));
+	std::cout << "Radar Thread Run" << std::endl;
+}
+
+void iv::perception::RadarSensor::stop()
+{
+	thread_sensor_run_->interrupt();
+	thread_sensor_run_->join();
+	std::cout << "Radar Thread Stop" << std::endl;
+}
+
+bool iv::perception::RadarSensor::isRunning() const
+{
+	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
+}
+

src/decition/common/perception/sensor_radar.h

@@ -1,37 +1,37 @@
-#pragma once
-#ifndef _IV_PERCEPTION_SENSOR_RADAR_
-#define _IV_PERCEPTION_SENSOR_RADAR_
-
-#include <common/boost.h>
-#include <perception/sensor.h>
-
-#include "radarobjectarray.pb.h"
-
-#include  "modulecomm.h"
-namespace iv {
-	namespace perception {
-		class RadarSensor : public iv::perception::Sensor {
-		public:
-			RadarSensor();
-			~RadarSensor();
-			// 通过 Sensor 继承
-			virtual void start() override;
-			virtual void stop() override;
-			virtual bool isRunning() const override;
-			virtual void processSensor() override;
-
-			/* 毫米波雷达障碍物*/
-			typedef void
-			(sig_cb_radar_sensor_obstacle)(iv::ObsRadar);
-
-		private:
-			boost::signals2::signal<sig_cb_radar_sensor_obstacle>* signal_radar_obstacle;//毫米波雷达障碍物信号
-
-        private:
-            void * mparadar;
-        public:
-            void UpdateRADAR(iv::radar::radarobjectarray xarray);
-		};
-	}
-}
-#endif // !_IV_PERCEPTION_SENSOR_RADAR_
+#pragma once
+#ifndef _IV_PERCEPTION_SENSOR_RADAR_
+#define _IV_PERCEPTION_SENSOR_RADAR_
+
+#include <common/boost.h>
+#include <perception/sensor.h>
+
+#include "radarobjectarray.pb.h"
+
+#include  "modulecomm.h"
+namespace iv {
+	namespace perception {
+		class RadarSensor : public iv::perception::Sensor {
+		public:
+			RadarSensor();
+			~RadarSensor();
+			// 通过 Sensor 继承
+			virtual void start() override;
+			virtual void stop() override;
+			virtual bool isRunning() const override;
+			virtual void processSensor() override;
+
+			/* 毫米波雷达障碍物*/
+			typedef void
+			(sig_cb_radar_sensor_obstacle)(iv::ObsRadar);
+
+		private:
+			boost::signals2::signal<sig_cb_radar_sensor_obstacle>* signal_radar_obstacle;//毫米波雷达障碍物信号
+
+        private:
+            void * mparadar;
+        public:
+            void UpdateRADAR(iv::radar::radarobjectarray xarray);
+		};
+	}
+}
+#endif // !_IV_PERCEPTION_SENSOR_RADAR_

src/decition/common/perception_sf/eyes.cpp

@@ -1,135 +1,135 @@
-#include <perception_sf/eyes.h>
-#include <time.h>
-
-iv::perception::Eyes::Eyes() {
-    mgpsindex = 0;
-    mgpsreadindex = 0;
-
-}
-iv::perception::Eyes::~Eyes() {
-
-}
-
-void iv::perception::Eyes::inialize() {
-
-    sensor_lidar = new iv::perception::LiDARSensor();
-    sensor_radar = new iv::perception::RadarSensor();
-    sensor_camera = new iv::perception::CameraSensor();
-    sensor_gps = new iv::perception::GPSSensor();
-
-
-    sensor_lidar->registerCallBack<iv::perception::LiDARSensor::sig_cb_lidar_sensor_obstacle_grid>(boost::bind(&iv::perception::Eyes::cb_lidar_grid, this, _1));
-
-
-
-    //sensor_radar->registerCallBack<iv::perception::RadarSensor::sig_cb_radar_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_radar, this, _1));
-    //sensor_camera->registerCallBack<iv::perception::CameraSensor::sig_cb_camera_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_camera, this, _1));
-    sensor_gps->registerCallBack<iv::perception::GPSSensor::sig_cb_gps_sensor_data>(boost::bind(&iv::perception::Eyes::cb_gps, this, _1));
-    sensor_lidar->start();
-    sensor_radar->start();
-    sensor_camera->start();
-    sensor_gps->start();
-    obs_lidar_gridptr_ = NULL;
-    obs_camera_ = NULL;
-}
-
-void iv::perception::Eyes::cb_lidar_grid(iv::LidarGridPtr obs) {
-    mutex_.lock();
-    if (obs_lidar_gridptr_ != NULL)
-    {
-        free(obs_lidar_gridptr_);
-    }
-    obs_lidar_gridptr_ = obs;
-    obs = NULL;
-    mutex_.unlock();
-}
-
-void iv::perception::Eyes::cb_radar(iv::ObsRadar obs) {
-    boost::mutex::scoped_lock(mutex_);
-    obs_radar_ = obs;
-}
-
-void iv::perception::Eyes::cb_camera(iv::CameraInfoPtr obs) {
-    mutex_.lock();
-    //boost::mutex::scoped_lock(mutex_);
-    if (obs_camera_ != NULL)
-    {
-        free(obs_camera_);
-    }
-    obs_camera_ = obs;
-    obs = NULL;
-    mutex_.unlock();
-}
-
-void iv::perception::Eyes::cb_gps(iv::GPSData gps_data) {
-    if((gps_data->gps_lat<0.01)||(gps_data->gps_lat>90))return;
-    boost::mutex::scoped_lock(mutex_);
-//    gps_ins_data_ = gps_data;
-
-    if(gps_data == NULL)return;
-    iv::GPSData data(new iv::GPS_INS);
-    if(data == NULL)return;
-    *data = *gps_data;
-    gps_ins_data_ = data;
-    mgpsindex++;
-
-}
-
-bool iv::perception::Eyes::isAllSensorRunning() {
-    return true;
-    bool bx;
-    //bx = sensor_lidar->isRunning() && sensor_radar->isRunning() && sensor_camera->isRunning() && sensor_gps->isRunning();
-    bx = sensor_gps->isRunning();
-//	ODS("Get Running time = %d", GetTickCount() - ticks);
-    return bx;
-}
-
-void iv::perception::Eyes::stopAllSensor() {
-    sensor_lidar->stop();
-    sensor_radar->stop();
-    sensor_camera->stop();
-    sensor_gps->stop();
-}
-
-void iv::perception::Eyes::getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& brain_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr) {
-    //brain_obs_camera = NULL;
-    brain_gps_data = NULL;
-    brain_obs_radar = NULL;
-    while (true) {
-        if (mutex_.try_lock()) {
-//            ServiceLidar.copylidarobsto(brain_obs_lidar_gridptr);
-            if(obs_lidar_gridptr_ != NULL)
-            {
-                brain_obs_lidar_gridptr  = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
-                memcpy(brain_obs_lidar_gridptr,obs_lidar_gridptr_,sizeof(Obs_grid[iv::grx][iv::gry]));
-             //   brain_obs_lidar_gridptr = obs_lidar_gridptr_;
-                free(obs_lidar_gridptr_);
-                obs_lidar_gridptr_ = NULL;
-            }
-            obs_lidar_gridptr_ = NULL;
-
- //           obs_radar_.swap(brain_obs_radar);
-            brain_obs_radar = NULL;
-
-            //obs_camera_.swap(brain_obs_camera);
-            brain_obs_camera = NULL;
-
-            if(mgpsindex != mgpsreadindex)
-  //          if(gps_ins_data_ != NULL)
-            {
-                iv::GPSData data(new iv::GPS_INS);
-                *data = *gps_ins_data_;
-                brain_gps_data =  data;
-                mgpsreadindex = mgpsindex;
-            }
-
-//            brain_gps_data = gps_ins_data_;
-//            gps_ins_data_ = NULL;
-
-
-            obs_camera_ = NULL;
-            mutex_.unlock();
-            break;
-        }
-    }
-}
+#include <perception_sf/eyes.h>
+#include <time.h>
+
+iv::perception::Eyes::Eyes() {
+    mgpsindex = 0;
+    mgpsreadindex = 0;
+
+}
+iv::perception::Eyes::~Eyes() {
+
+}
+
+void iv::perception::Eyes::inialize() {
+
+    sensor_lidar = new iv::perception::LiDARSensor();
+    sensor_radar = new iv::perception::RadarSensor();
+    sensor_camera = new iv::perception::CameraSensor();
+    sensor_gps = new iv::perception::GPSSensor();
+
+
+    sensor_lidar->registerCallBack<iv::perception::LiDARSensor::sig_cb_lidar_sensor_obstacle_grid>(boost::bind(&iv::perception::Eyes::cb_lidar_grid, this, _1));
+
+
+
+    //sensor_radar->registerCallBack<iv::perception::RadarSensor::sig_cb_radar_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_radar, this, _1));
+    //sensor_camera->registerCallBack<iv::perception::CameraSensor::sig_cb_camera_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_camera, this, _1));
+    sensor_gps->registerCallBack<iv::perception::GPSSensor::sig_cb_gps_sensor_data>(boost::bind(&iv::perception::Eyes::cb_gps, this, _1));
+    sensor_lidar->start();
+    sensor_radar->start();
+    sensor_camera->start();
+    sensor_gps->start();
+    obs_lidar_gridptr_ = NULL;
+    obs_camera_ = NULL;
+}
+
+void iv::perception::Eyes::cb_lidar_grid(iv::LidarGridPtr obs) {
+    mutex_.lock();
+    if (obs_lidar_gridptr_ != NULL)
+    {
+        free(obs_lidar_gridptr_);
+    }
+    obs_lidar_gridptr_ = obs;
+    obs = NULL;
+    mutex_.unlock();
+}
+
+void iv::perception::Eyes::cb_radar(iv::ObsRadar obs) {
+    boost::mutex::scoped_lock(mutex_);
+    obs_radar_ = obs;
+}
+
+void iv::perception::Eyes::cb_camera(iv::CameraInfoPtr obs) {
+    mutex_.lock();
+    //boost::mutex::scoped_lock(mutex_);
+    if (obs_camera_ != NULL)
+    {
+        free(obs_camera_);
+    }
+    obs_camera_ = obs;
+    obs = NULL;
+    mutex_.unlock();
+}
+
+void iv::perception::Eyes::cb_gps(iv::GPSData gps_data) {
+    if((gps_data->gps_lat<0.01)||(gps_data->gps_lat>90))return;
+    boost::mutex::scoped_lock(mutex_);
+//    gps_ins_data_ = gps_data;
+
+    if(gps_data == NULL)return;
+    iv::GPSData data(new iv::GPS_INS);
+    if(data == NULL)return;
+    *data = *gps_data;
+    gps_ins_data_ = data;
+    mgpsindex++;
+
+}
+
+bool iv::perception::Eyes::isAllSensorRunning() {
+    return true;
+    bool bx;
+    //bx = sensor_lidar->isRunning() && sensor_radar->isRunning() && sensor_camera->isRunning() && sensor_gps->isRunning();
+    bx = sensor_gps->isRunning();
+//	ODS("Get Running time = %d", GetTickCount() - ticks);
+    return bx;
+}
+
+void iv::perception::Eyes::stopAllSensor() {
+    sensor_lidar->stop();
+    sensor_radar->stop();
+    sensor_camera->stop();
+    sensor_gps->stop();
+}
+
+void iv::perception::Eyes::getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& brain_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr) {
+    //brain_obs_camera = NULL;
+    brain_gps_data = NULL;
+    brain_obs_radar = NULL;
+    while (true) {
+        if (mutex_.try_lock()) {
+//            ServiceLidar.copylidarobsto(brain_obs_lidar_gridptr);
+            if(obs_lidar_gridptr_ != NULL)
+            {
+                brain_obs_lidar_gridptr  = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+                memcpy(brain_obs_lidar_gridptr,obs_lidar_gridptr_,sizeof(Obs_grid[iv::grx][iv::gry]));
+             //   brain_obs_lidar_gridptr = obs_lidar_gridptr_;
+                free(obs_lidar_gridptr_);
+                obs_lidar_gridptr_ = NULL;
+            }
+            obs_lidar_gridptr_ = NULL;
+
+ //           obs_radar_.swap(brain_obs_radar);
+            brain_obs_radar = NULL;
+
+            //obs_camera_.swap(brain_obs_camera);
+            brain_obs_camera = NULL;
+
+            if(mgpsindex != mgpsreadindex)
+  //          if(gps_ins_data_ != NULL)
+            {
+                iv::GPSData data(new iv::GPS_INS);
+                *data = *gps_ins_data_;
+                brain_gps_data =  data;
+                mgpsreadindex = mgpsindex;
+            }
+
+//            brain_gps_data = gps_ins_data_;
+//            gps_ins_data_ = NULL;
+
+
+            obs_camera_ = NULL;
+            mutex_.unlock();
+            break;
+        }
+    }
+}

src/decition/common/perception_sf/eyes.h

@@ -1,61 +1,61 @@
-#pragma once
-
-#ifndef _IV_PERCEPTION_EYES_
-#define _IV_PERCEPTION_EYES_
-
-#include <perception_sf/sensor_lidar.h>
-#include <perception_sf/sensor_radar.h>
-#include <perception_sf/sensor_camera.h>
-#include <perception_sf/sensor_gps.h>
-
-namespace iv {
-    namespace perception {
-        class Eyes
-        {
-        public:
-            Eyes();
-            ~Eyes();
-
-
-            void inialize();/* 初始化*/
-
-
-            bool isAllSensorRunning(); /* 所有传感器是否都在运行*/
-
-
-            void stopAllSensor(); /* 关闭所有传感器*/
-
-            void getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& bran_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr);
-
-            iv::ObsLiDAR obs_lidar_;
-            iv::LidarGridPtr obs_lidar_gridptr_;
-            iv::ObsRadar obs_radar_;
-            iv::CameraInfoPtr obs_camera_;
-            iv::GPSData gps_ins_data_;
-
-        private:
-
-            void cb_lidar(iv::ObsLiDAR obs);
-            void cb_lidar_grid(iv::LidarGridPtr obs);
-            void cb_radar(iv::ObsRadar obs);
-            void cb_camera(iv::CameraInfoPtr obs);
-            void cb_gps(iv::GPSData gps_data);
-
-            iv::perception::LiDARSensor* sensor_lidar;
-            iv::perception::RadarSensor* sensor_radar;
-            iv::perception::CameraSensor* sensor_camera;
-            iv::perception::GPSSensor* sensor_gps;
-
-            boost::mutex mutex_;
-
-            int ready1 = 0, ready2 = 0;
-
-            int mgpsindex;
-            int mgpsreadindex;
-        };
-
-    }
-}
-
-
-#endif // !_IV_PERCEPTION_EYES_
+#pragma once
+
+#ifndef _IV_PERCEPTION_EYES_
+#define _IV_PERCEPTION_EYES_
+
+#include <perception_sf/sensor_lidar.h>
+#include <perception_sf/sensor_radar.h>
+#include <perception_sf/sensor_camera.h>
+#include <perception_sf/sensor_gps.h>
+
+namespace iv {
+    namespace perception {
+        class Eyes
+        {
+        public:
+            Eyes();
+            ~Eyes();
+
+
+            void inialize();/* 初始化*/
+
+
+            bool isAllSensorRunning(); /* 所有传感器是否都在运行*/
+
+
+            void stopAllSensor(); /* 关闭所有传感器*/
+
+            void getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& bran_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr);
+
+            iv::ObsLiDAR obs_lidar_;
+            iv::LidarGridPtr obs_lidar_gridptr_;
+            iv::ObsRadar obs_radar_;
+            iv::CameraInfoPtr obs_camera_;
+            iv::GPSData gps_ins_data_;
+
+        private:
+
+            void cb_lidar(iv::ObsLiDAR obs);
+            void cb_lidar_grid(iv::LidarGridPtr obs);
+            void cb_radar(iv::ObsRadar obs);
+            void cb_camera(iv::CameraInfoPtr obs);
+            void cb_gps(iv::GPSData gps_data);
+
+            iv::perception::LiDARSensor* sensor_lidar;
+            iv::perception::RadarSensor* sensor_radar;
+            iv::perception::CameraSensor* sensor_camera;
+            iv::perception::GPSSensor* sensor_gps;
+
+            boost::mutex mutex_;
+
+            int ready1 = 0, ready2 = 0;
+
+            int mgpsindex;
+            int mgpsreadindex;
+        };
+
+    }
+}
+
+
+#endif // !_IV_PERCEPTION_EYES_

src/decition/common/perception_sf/impl_camera.cpp

@@ -1,43 +1,43 @@
-/*
-* 在此方法中处理摄像头的打开、处理数据、关闭等
-* 识别到障碍物时 触发signal_camera_obstacle
-*
-*/
-#include <perception_sf/sensor_camera.h>
-
-void iv::perception::CameraSensor::processSensor() {
-	std::cout << "camera process start" << std::endl;
-	//打开激光雷达
-	//障碍物识别
-	iv::ObsCamera obs(new std::vector<iv::ObstacleBasic>);
-	iv::ObstacleBasic t = {  };
-	obs->push_back(t);
-	//ODS("\n\n------>thread camera: %X\n\n", GetCurrentThreadId());
-	while (true) {
-		srand(time(NULL));
-		if (rand() % 11 == 0) {
-			obs->at(0).valid = true;
-			obs->at(0).nomal_x = rand() % 5;
-			obs->at(0).nomal_y = rand() / 6;
-			obs->at(0).nomal_z = rand() / 7;
-
-            //signal_camera_obstacle->operator()(obs);//触发
-
-
-			//std::cout << "Camera Dectected: " << obs->at(0).nomal_x << " " << obs->at(0).nomal_y << " " << obs->at(0).nomal_z << std::endl;
-
-		}
-		/*if (rand() % 9 == 0)
-			break;*/
-#ifdef linux
-            sleep(1);
-#endif
-#ifdef WIN32
-            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
-  //          Sleep(1000);
-#endif
-        //boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
-	}
-
-	std::cout << "camera process end" << std::endl;
-}
+/*
+* 在此方法中处理摄像头的打开、处理数据、关闭等
+* 识别到障碍物时 触发signal_camera_obstacle
+*
+*/
+#include <perception_sf/sensor_camera.h>
+
+void iv::perception::CameraSensor::processSensor() {
+	std::cout << "camera process start" << std::endl;
+	//打开激光雷达
+	//障碍物识别
+	iv::ObsCamera obs(new std::vector<iv::ObstacleBasic>);
+	iv::ObstacleBasic t = {  };
+	obs->push_back(t);
+	//ODS("\n\n------>thread camera: %X\n\n", GetCurrentThreadId());
+	while (true) {
+		srand(time(NULL));
+		if (rand() % 11 == 0) {
+			obs->at(0).valid = true;
+			obs->at(0).nomal_x = rand() % 5;
+			obs->at(0).nomal_y = rand() / 6;
+			obs->at(0).nomal_z = rand() / 7;
+
+            //signal_camera_obstacle->operator()(obs);//触发
+
+
+			//std::cout << "Camera Dectected: " << obs->at(0).nomal_x << " " << obs->at(0).nomal_y << " " << obs->at(0).nomal_z << std::endl;
+
+		}
+		/*if (rand() % 9 == 0)
+			break;*/
+#ifdef linux
+            sleep(1);
+#endif
+#ifdef WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+  //          Sleep(1000);
+#endif
+        //boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
+	}
+
+	std::cout << "camera process end" << std::endl;
+}

src/decition/common/perception_sf/impl_gps.cpp

@@ -1,346 +1,346 @@
-#include <perception_sf/sensor_gps.h>
-#include <decition/gnss_coordinate_convert.h>
-#include <control/can.h>
-#include <QDebug>
-#include <QNetworkDatagram>
-
-static double   cast_8_byte_to_double(const uint8_t *b);
-static float    cast_4_byte_to_float(const uint8_t *b);
-static  int32_t cast_3_byte_to_int32(const uint8_t *b);
-
-void iv::perception::GPSSensor::processSensor()
-{
-
-    return;
-	//todo  GPS/惯导 设备接口 对接
-    /*Initialize udp server, listen on port 3000.*/
-    /*int sockfd;
-    struct sockaddr_in addr;
-    socklen_t addrlen;
-    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
-    if (sockfd < 0)
-    {
-       printf("socket failed\n");
-       exit(EXIT_FAILURE);
-    }
-    addrlen = sizeof(struct sockaddr_in);
-    bzero(&addr, addrlen);
-    addr.sin_family = AF_INET;
-    addr.sin_addr.s_addr = htonl(INADDR_ANY);
-    addr.sin_port = htons(3000);
-    if (bind(sockfd, (struct sockaddr*)(&addr), addrlen) < 0)
-    {
-       printf("bind fail\n");
-       exit(EXIT_FAILURE);
-    }*/
-    QUdpSocket *udpsocket;//summer
-    udpsocket = new QUdpSocket();
-    udpsocket->bind(QHostAddress::Any, 3000);
- //   udpsocket->bind(3000);
- /*   if(!udpsocket->waitForConnected())
-    {
-        printf("bind fail\n");
-        exit(EXIT_FAILURE);
-    }*/
-    char *Buf = new char[100] ;//summer unsigned char - char
-    memset(Buf,0,100);
-    unsigned char recvBuf[100] = {0};
-
-	/*UDP initialization done. Next prepare for receiving data.*/
-
-	iv::GPSData data(new iv::GPS_INS);
-	int x, y, z;
-
-	//iv::CarStatus currentCarStatus;
-	//ODS("\n\n\n------>thread gps: %X\n\n\n\n", GetCurrentThreadId());
-	while (true) {
-        int rec = 0;
-        if(udpsocket->hasPendingDatagrams())
-        {
-  //          std::cout<<"have data."<<std::endl;
-            QNetworkDatagram datagram = udpsocket->receiveDatagram();
-            QByteArray ba = datagram.data();
-            if(ba.size() != NOUTPUT_PACKET_LENGTH)
-            {
-                continue;
-            }
-            else
-            {
-                rec = NOUTPUT_PACKET_LENGTH;
-                memcpy(recvBuf,ba.data(),NOUTPUT_PACKET_LENGTH);
-            }
-            datagram.clear();
-        }
-        else
-        {
-#ifdef Q_OS_LINUX
-            usleep(1000);
-#endif
-#ifdef WIN32
-            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
-//            Sleep(5);
-#endif
-            continue;
-//            std::cout<<"running."<<std::endl;
- //           std::this_thread::sleep_for(std::chrono::milliseconds(1));
-        }
-/*        if(udpsocket->waitForReadyRead())
-            rec = udpsocket->read(Buf,100);
-        //recvBuf = (unsigned char*)Buf;
-        convertStrToUnChar(Buf,recvBuf);//summer */
-        //int rec = recvfrom(sockfd, recvBuf, 100, 0, (struct sockaddr *)(&addr), &addrlen);
-		/*Check the receiving data*/
-		//There must be 72 bytes in one package.
-		if (rec != NOUTPUT_PACKET_LENGTH) {
-			printf("ERR: rec must be 72 bytes\n");
-			continue;
-		}
-
-		if (recvBuf[PI_SYNC] != NCOM_SYNC) {
-			printf("ERR: head always be 0xE7\n");
-			continue;
-		}
-
-        data->ins_status = recvBuf[PI_INS_NAV_MODE];
-        if(data->ins_status == 0x0B)continue;
-
-        //31.1150882 121.2211320 356.9
-        //31.1150844 121.2210996 356.9 VV7_3
-
-		/*  Start decoding
-		Details in ncomman.pdf
-		Byte		| 0	   | 1-20	| 21         | 22      | 23-60  | 61      | 62       | 63-70  | 71
-		Description	| Sync | BatchA | NaviStatus | ChkSum1 | BatchB | ChkSum2 | StatusCh | BatchS | ChkSum3
-		*/
-
-		//start decoding -> Batch A
-        //Time is transmitted as milliseconds into the current GPS minute. The range is 0–59, 999 ms.
-
-		/*
-		Acceleration x is the host object’s acceleration in the x-direction
-		(i.e.after the IMU to host attitude matrix has been applied).
-		It is a signed word in units of 1 × 10^(−4)  m / (s^2)
-		*/
-		x = cast_3_byte_to_int32(recvBuf + PI_ACCEL_X);
-		y = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Y);
-		z = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Z);
-		if (x != INV_INT_24) data->accel_x = x * ACC2MPS2;
-		if (y != INV_INT_24) data->accel_y = y * ACC2MPS2;
-		if (z != INV_INT_24) data->accel_z = z * ACC2MPS2;
-
-		/*
-		Angular rate x is the host object’s angular rate about its x-axis
-		(i.e. after the IMU to host attitude matrix has been applied).
-		It is a signed word in units of 1 × 10−5 radians/s
-		*/
-		x = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_X);
-		y = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Y);
-		z = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Z);
-		if (x != INV_INT_24) data->ang_rate_x = x * (RATE2RPS * RAD2DEG);
-		if (y != INV_INT_24) data->ang_rate_y = y * (RATE2RPS * RAD2DEG);
-		if (z != INV_INT_24) data->ang_rate_z = z * (RATE2RPS * RAD2DEG);
-
-		/*
-		The navigation status byte value should be 0–7, 10 or 20–22 to be valid
-		for customer use. See page 10. A value of 11 indicates the packet
-		follows NCOM structure-B and should be ignored.
-		*/
-		data->ins_status = recvBuf[PI_INS_NAV_MODE];
-		//navi_status refer to imu status 2 3 4.
-		//check this out in page 11 table 5.
-
-		/*
-		Checksum 1 allows the software to verify the integrity of bytes 1–21.
-		The sync byte is ignored. In low-latency applications the inertial
-		measurements in Batch A can be used to update a previous solution
-		without waiting for the rest of the packet to be received. Contact Oxford
-		Technical Solutions for source code to perform this function.
-        */
-        //31.1150796 121.2211047 355.8
-
-		//start decoding Batch B
-		/*
-		Position, orientation and velocity output. See Table 6, for a detailed
-		description.
-		*/
-		/*Byte23-30. The Latitude of the INS. It is a double in units of radians.*/
-		/*Byte31-38. Longitude of the INS. It is a double in units of radians.*/
-		/*Byte39-42. Altitude of the INS. It is a float in units of metres.*/
-		/*double latitude;
-		double longitude;
-		memcpy(&latitude, recvBuf + 23, sizeof(double));
-		memcpy(&longitude, recvBuf + 31, sizeof(double));
-		latitude = latitude * 180.0 / M_PI;
-		longitude = longitude * 180.0 / M_PI;
-		printf("lat= %.20lf   lon= %.20lf  | sizeof(float)=%d\n", latitude, longitude, sizeof(float));
-		*/
-		data->gps_lat = cast_8_byte_to_double(recvBuf + PI_POS_LAT) * RAD2DEG;
-		data->gps_lng = cast_8_byte_to_double(recvBuf + PI_POS_LON) * RAD2DEG;
-		data->gps_z = cast_4_byte_to_float(recvBuf + PI_POS_ALT);
-
-		/*Byte43-45. Byte46-48. Byte49-51. Velocity in units of 1 × 10^−4 m/s.*/
-		x = cast_3_byte_to_int32(recvBuf + PI_VEL_N);
-		y = cast_3_byte_to_int32(recvBuf + PI_VEL_E);
-		z = cast_3_byte_to_int32(recvBuf + PI_VEL_D);
-		if (x != INV_INT_24) data->vel_N = x * VEL2MPS;
-		if (y != INV_INT_24) data->vel_E = y * VEL2MPS;
-		if (z != INV_INT_24) data->vel_D = z * VEL2MPS;
-
-		/*Byte52-54. Heading in units of 1 × 10−6 radians. Range ±pi.*/
-		/*Byte55-57. Pitch in units of 1 × 10−6 radians. Range ±pi/2.*/
-		/*Byte58-60. Roll in units of 1 × 10−6 radians. Range ±pi.*/
-		x = cast_3_byte_to_int32(recvBuf + PI_ORIEN_H);
-		y = cast_3_byte_to_int32(recvBuf + PI_ORIEN_P);
-		z = cast_3_byte_to_int32(recvBuf + PI_ORIEN_R);
-		if (x != INV_INT_24) data->ins_heading_angle = x * (ANG2RAD * RAD2DEG);
-		if (y != INV_INT_24) data->ins_pitch_angle = y * (ANG2RAD * RAD2DEG);
-		if (z != INV_INT_24) data->ins_roll_angle = z * (ANG2RAD * RAD2DEG);
-#ifdef VV7_2
-        data->ins_heading_angle = data->ins_heading_angle+1.9;
-#endif
-#ifdef VV7_3
-        data->ins_heading_angle = data->ins_heading_angle-1.0;
-#endif
-        if(data->ins_heading_angle>360)data->ins_heading_angle = data->ins_heading_angle-360;
-		if (data->ins_heading_angle < 0.0)
-            data->ins_heading_angle += 360.0;
-
-		if (recvBuf[PI_CHANNEL_INDEX] == PI_CHANNEL_INDEX) {
-			data->rtk_status = recvBuf[PI_RTK_STATUS];
-			data->gps_satelites_num = recvBuf[PI_SAT_NUM];
-		}
-
-        ServiceCarStatus.mRTKStatus = data->rtk_status;
-
-		if (data->ins_status != 4) {
-			data->valid = IMU_STATUS_ERR;
-		} else if (data->rtk_status != 3 && data->rtk_status != 5 && data->rtk_status != 6) {
-			data->valid = RTK_STATUS_ERR;
-		} else {
-			data->valid = RTK_IMU_OK;
-		}
-
-        GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-		
-        ////从can中读取当前车速
-        //data->speed = ServiceCarStatus.speed;
-        if(data->ins_status != 0x0B)data->speed = sqrt(data->vel_N*data->vel_N + data->vel_E * data->vel_E)*3.6;
-        else continue;
-
-        if(data->gps_lat<0||data->gps_lat>90){
-            continue;
-        }
-
-        ServiceCarStatus.speed = data->speed;
- //       qDebug("speed x is %g",ServiceCarStatus.speed);
-		if(data->ins_status == 4)
-            signal_gps_data->operator()(data);	//传输数据
-		ServiceCarStatus.location->gps_lat = data->gps_lat;
-		ServiceCarStatus.location->gps_lng = data->gps_lng;
-		ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
-		ServiceCarStatus.location->gps_x = data->gps_x;
-        ServiceCarStatus.location->gps_y = data->gps_y;
-        ServiceCarStatus.location->rtk_status = data->rtk_status;
-        ServiceCarStatus.location->ins_status = data->ins_status;
- //       std::cout<<"lat = "<<ServiceCarStatus.location->gps_lat<<std::endl;
-    }
-    //close(sockfd);
-    udpsocket->close();
-}
-
-static double cast_8_byte_to_double(const uint8_t *b)
-{
-	union { double x; uint8_t c[8]; } u;
-	u.c[0] = b[0];
-	u.c[1] = b[1];
-	u.c[2] = b[2];
-	u.c[3] = b[3];
-	u.c[4] = b[4];
-	u.c[5] = b[5];
-	u.c[6] = b[6];
-	u.c[7] = b[7];
-	return u.x;
-}
-
-static float cast_4_byte_to_float(const uint8_t *b)
-{
-	union { float x; uint8_t c[4]; } u;
-	u.c[0] = b[0];
-	u.c[1] = b[1];
-	u.c[2] = b[2];
-	u.c[3] = b[3];
-	return u.x;
-}
-
-static int32_t cast_3_byte_to_int32(const uint8_t *b)
-{
-	union { int32_t x; uint8_t c[4]; } u;
-	u.c[1] = b[0];
-	u.c[2] = b[1];
-	u.c[3] = b[2];
-	return u.x >> 8;
-}
-
-
-void iv::perception::GPSSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
-{
-    int i = strlen(str), j = 0, counter = 0;
-    char c[2];
-    unsigned int bytes[2];
-
-    for (j = 0; j < i; j += 2)
-    {
-        if(0 == j % 2)
-        {
-            c[0] = str[j];
-            c[1] = str[j + 1];
-            sscanf(c, "%02x" , &bytes[0]);
-            UnChar[counter] = bytes[0];
-            counter++;
-        }
-    }
-}
-
-void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
-{
-    iv::GPSData data(new iv::GPS_INS);
-    data->ins_status = xgpsimu.ins_state();
-    data->rtk_status = xgpsimu.rtk_state();
-
-    data->accel_x = xgpsimu.acce_x();
-    data->accel_y = xgpsimu.acce_y();
-    data->accel_z = xgpsimu.acce_z();
-    data->ang_rate_x = xgpsimu.gyro_x();
-    data->ang_rate_y = xgpsimu.gyro_y();
-    data->ang_rate_z = xgpsimu.gyro_z();
-
-    data->gps_lat = xgpsimu.lat();
-    data->gps_lng = xgpsimu.lon();
-    data->gps_z = xgpsimu.height();
-
-    data->ins_heading_angle = xgpsimu.heading();
-    data->ins_pitch_angle = xgpsimu.pitch();
-    data->ins_roll_angle = xgpsimu.roll();
-
-    data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
-
-    GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-
-    ServiceCarStatus.mRTKStatus = data->rtk_status;
-
-    ServiceCarStatus.mfGPSAcc = xgpsimu.acc_calc();
-
-    ServiceCarStatus.speed = data->speed;
-//       qDebug("speed x is %g",ServiceCarStatus.speed);
-    if(data->ins_status == 4)
-        signal_gps_data->operator()(data);	//传输数据
-
-    ServiceCarStatus.location->gps_lat = data->gps_lat;
-    ServiceCarStatus.location->gps_lng = data->gps_lng;
-    ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
-    ServiceCarStatus.location->gps_x = data->gps_x;
-    ServiceCarStatus.location->gps_y = data->gps_y;
-    ServiceCarStatus.location->rtk_status = data->rtk_status;
-    ServiceCarStatus.location->ins_status = data->ins_status;
-
-}
+#include <perception_sf/sensor_gps.h>
+#include <decition/gnss_coordinate_convert.h>
+#include <control/can.h>
+#include <QDebug>
+#include <QNetworkDatagram>
+
+static double   cast_8_byte_to_double(const uint8_t *b);
+static float    cast_4_byte_to_float(const uint8_t *b);
+static  int32_t cast_3_byte_to_int32(const uint8_t *b);
+
+void iv::perception::GPSSensor::processSensor()
+{
+
+    return;
+	//todo  GPS/惯导 设备接口 对接
+    /*Initialize udp server, listen on port 3000.*/
+    /*int sockfd;
+    struct sockaddr_in addr;
+    socklen_t addrlen;
+    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+    if (sockfd < 0)
+    {
+       printf("socket failed\n");
+       exit(EXIT_FAILURE);
+    }
+    addrlen = sizeof(struct sockaddr_in);
+    bzero(&addr, addrlen);
+    addr.sin_family = AF_INET;
+    addr.sin_addr.s_addr = htonl(INADDR_ANY);
+    addr.sin_port = htons(3000);
+    if (bind(sockfd, (struct sockaddr*)(&addr), addrlen) < 0)
+    {
+       printf("bind fail\n");
+       exit(EXIT_FAILURE);
+    }*/
+    QUdpSocket *udpsocket;//summer
+    udpsocket = new QUdpSocket();
+    udpsocket->bind(QHostAddress::Any, 3000);
+ //   udpsocket->bind(3000);
+ /*   if(!udpsocket->waitForConnected())
+    {
+        printf("bind fail\n");
+        exit(EXIT_FAILURE);
+    }*/
+    char *Buf = new char[100] ;//summer unsigned char - char
+    memset(Buf,0,100);
+    unsigned char recvBuf[100] = {0};
+
+	/*UDP initialization done. Next prepare for receiving data.*/
+
+	iv::GPSData data(new iv::GPS_INS);
+	int x, y, z;
+
+	//iv::CarStatus currentCarStatus;
+	//ODS("\n\n\n------>thread gps: %X\n\n\n\n", GetCurrentThreadId());
+	while (true) {
+        int rec = 0;
+        if(udpsocket->hasPendingDatagrams())
+        {
+  //          std::cout<<"have data."<<std::endl;
+            QNetworkDatagram datagram = udpsocket->receiveDatagram();
+            QByteArray ba = datagram.data();
+            if(ba.size() != NOUTPUT_PACKET_LENGTH)
+            {
+                continue;
+            }
+            else
+            {
+                rec = NOUTPUT_PACKET_LENGTH;
+                memcpy(recvBuf,ba.data(),NOUTPUT_PACKET_LENGTH);
+            }
+            datagram.clear();
+        }
+        else
+        {
+#ifdef Q_OS_LINUX
+            usleep(1000);
+#endif
+#ifdef WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+//            Sleep(5);
+#endif
+            continue;
+//            std::cout<<"running."<<std::endl;
+ //           std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+/*        if(udpsocket->waitForReadyRead())
+            rec = udpsocket->read(Buf,100);
+        //recvBuf = (unsigned char*)Buf;
+        convertStrToUnChar(Buf,recvBuf);//summer */
+        //int rec = recvfrom(sockfd, recvBuf, 100, 0, (struct sockaddr *)(&addr), &addrlen);
+		/*Check the receiving data*/
+		//There must be 72 bytes in one package.
+		if (rec != NOUTPUT_PACKET_LENGTH) {
+			printf("ERR: rec must be 72 bytes\n");
+			continue;
+		}
+
+		if (recvBuf[PI_SYNC] != NCOM_SYNC) {
+			printf("ERR: head always be 0xE7\n");
+			continue;
+		}
+
+        data->ins_status = recvBuf[PI_INS_NAV_MODE];
+        if(data->ins_status == 0x0B)continue;
+
+        //31.1150882 121.2211320 356.9
+        //31.1150844 121.2210996 356.9 VV7_3
+
+		/*  Start decoding
+		Details in ncomman.pdf
+		Byte		| 0	   | 1-20	| 21         | 22      | 23-60  | 61      | 62       | 63-70  | 71
+		Description	| Sync | BatchA | NaviStatus | ChkSum1 | BatchB | ChkSum2 | StatusCh | BatchS | ChkSum3
+		*/
+
+		//start decoding -> Batch A
+        //Time is transmitted as milliseconds into the current GPS minute. The range is 0–59, 999 ms.
+
+		/*
+		Acceleration x is the host object’s acceleration in the x-direction
+		(i.e.after the IMU to host attitude matrix has been applied).
+		It is a signed word in units of 1 × 10^(−4)  m / (s^2)
+		*/
+		x = cast_3_byte_to_int32(recvBuf + PI_ACCEL_X);
+		y = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Y);
+		z = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Z);
+		if (x != INV_INT_24) data->accel_x = x * ACC2MPS2;
+		if (y != INV_INT_24) data->accel_y = y * ACC2MPS2;
+		if (z != INV_INT_24) data->accel_z = z * ACC2MPS2;
+
+		/*
+		Angular rate x is the host object’s angular rate about its x-axis
+		(i.e. after the IMU to host attitude matrix has been applied).
+		It is a signed word in units of 1 × 10−5 radians/s
+		*/
+		x = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_X);
+		y = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Y);
+		z = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Z);
+		if (x != INV_INT_24) data->ang_rate_x = x * (RATE2RPS * RAD2DEG);
+		if (y != INV_INT_24) data->ang_rate_y = y * (RATE2RPS * RAD2DEG);
+		if (z != INV_INT_24) data->ang_rate_z = z * (RATE2RPS * RAD2DEG);
+
+		/*
+		The navigation status byte value should be 0–7, 10 or 20–22 to be valid
+		for customer use. See page 10. A value of 11 indicates the packet
+		follows NCOM structure-B and should be ignored.
+		*/
+		data->ins_status = recvBuf[PI_INS_NAV_MODE];
+		//navi_status refer to imu status 2 3 4.
+		//check this out in page 11 table 5.
+
+		/*
+		Checksum 1 allows the software to verify the integrity of bytes 1–21.
+		The sync byte is ignored. In low-latency applications the inertial
+		measurements in Batch A can be used to update a previous solution
+		without waiting for the rest of the packet to be received. Contact Oxford
+		Technical Solutions for source code to perform this function.
+        */
+        //31.1150796 121.2211047 355.8
+
+		//start decoding Batch B
+		/*
+		Position, orientation and velocity output. See Table 6, for a detailed
+		description.
+		*/
+		/*Byte23-30. The Latitude of the INS. It is a double in units of radians.*/
+		/*Byte31-38. Longitude of the INS. It is a double in units of radians.*/
+		/*Byte39-42. Altitude of the INS. It is a float in units of metres.*/
+		/*double latitude;
+		double longitude;
+		memcpy(&latitude, recvBuf + 23, sizeof(double));
+		memcpy(&longitude, recvBuf + 31, sizeof(double));
+		latitude = latitude * 180.0 / M_PI;
+		longitude = longitude * 180.0 / M_PI;
+		printf("lat= %.20lf   lon= %.20lf  | sizeof(float)=%d\n", latitude, longitude, sizeof(float));
+		*/
+		data->gps_lat = cast_8_byte_to_double(recvBuf + PI_POS_LAT) * RAD2DEG;
+		data->gps_lng = cast_8_byte_to_double(recvBuf + PI_POS_LON) * RAD2DEG;
+		data->gps_z = cast_4_byte_to_float(recvBuf + PI_POS_ALT);
+
+		/*Byte43-45. Byte46-48. Byte49-51. Velocity in units of 1 × 10^−4 m/s.*/
+		x = cast_3_byte_to_int32(recvBuf + PI_VEL_N);
+		y = cast_3_byte_to_int32(recvBuf + PI_VEL_E);
+		z = cast_3_byte_to_int32(recvBuf + PI_VEL_D);
+		if (x != INV_INT_24) data->vel_N = x * VEL2MPS;
+		if (y != INV_INT_24) data->vel_E = y * VEL2MPS;
+		if (z != INV_INT_24) data->vel_D = z * VEL2MPS;
+
+		/*Byte52-54. Heading in units of 1 × 10−6 radians. Range ±pi.*/
+		/*Byte55-57. Pitch in units of 1 × 10−6 radians. Range ±pi/2.*/
+		/*Byte58-60. Roll in units of 1 × 10−6 radians. Range ±pi.*/
+		x = cast_3_byte_to_int32(recvBuf + PI_ORIEN_H);
+		y = cast_3_byte_to_int32(recvBuf + PI_ORIEN_P);
+		z = cast_3_byte_to_int32(recvBuf + PI_ORIEN_R);
+		if (x != INV_INT_24) data->ins_heading_angle = x * (ANG2RAD * RAD2DEG);
+		if (y != INV_INT_24) data->ins_pitch_angle = y * (ANG2RAD * RAD2DEG);
+		if (z != INV_INT_24) data->ins_roll_angle = z * (ANG2RAD * RAD2DEG);
+#ifdef VV7_2
+        data->ins_heading_angle = data->ins_heading_angle+1.9;
+#endif
+#ifdef VV7_3
+        data->ins_heading_angle = data->ins_heading_angle-1.0;
+#endif
+        if(data->ins_heading_angle>360)data->ins_heading_angle = data->ins_heading_angle-360;
+		if (data->ins_heading_angle < 0.0)
+            data->ins_heading_angle += 360.0;
+
+		if (recvBuf[PI_CHANNEL_INDEX] == PI_CHANNEL_INDEX) {
+			data->rtk_status = recvBuf[PI_RTK_STATUS];
+			data->gps_satelites_num = recvBuf[PI_SAT_NUM];
+		}
+
+        ServiceCarStatus.mRTKStatus = data->rtk_status;
+
+		if (data->ins_status != 4) {
+			data->valid = IMU_STATUS_ERR;
+		} else if (data->rtk_status != 3 && data->rtk_status != 5 && data->rtk_status != 6) {
+			data->valid = RTK_STATUS_ERR;
+		} else {
+			data->valid = RTK_IMU_OK;
+		}
+
+        GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+		
+        ////从can中读取当前车速
+        //data->speed = ServiceCarStatus.speed;
+        if(data->ins_status != 0x0B)data->speed = sqrt(data->vel_N*data->vel_N + data->vel_E * data->vel_E)*3.6;
+        else continue;
+
+        if(data->gps_lat<0||data->gps_lat>90){
+            continue;
+        }
+
+        ServiceCarStatus.speed = data->speed;
+ //       qDebug("speed x is %g",ServiceCarStatus.speed);
+		if(data->ins_status == 4)
+            signal_gps_data->operator()(data);	//传输数据
+		ServiceCarStatus.location->gps_lat = data->gps_lat;
+		ServiceCarStatus.location->gps_lng = data->gps_lng;
+		ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
+		ServiceCarStatus.location->gps_x = data->gps_x;
+        ServiceCarStatus.location->gps_y = data->gps_y;
+        ServiceCarStatus.location->rtk_status = data->rtk_status;
+        ServiceCarStatus.location->ins_status = data->ins_status;
+ //       std::cout<<"lat = "<<ServiceCarStatus.location->gps_lat<<std::endl;
+    }
+    //close(sockfd);
+    udpsocket->close();
+}
+
+static double cast_8_byte_to_double(const uint8_t *b)
+{
+	union { double x; uint8_t c[8]; } u;
+	u.c[0] = b[0];
+	u.c[1] = b[1];
+	u.c[2] = b[2];
+	u.c[3] = b[3];
+	u.c[4] = b[4];
+	u.c[5] = b[5];
+	u.c[6] = b[6];
+	u.c[7] = b[7];
+	return u.x;
+}
+
+static float cast_4_byte_to_float(const uint8_t *b)
+{
+	union { float x; uint8_t c[4]; } u;
+	u.c[0] = b[0];
+	u.c[1] = b[1];
+	u.c[2] = b[2];
+	u.c[3] = b[3];
+	return u.x;
+}
+
+static int32_t cast_3_byte_to_int32(const uint8_t *b)
+{
+	union { int32_t x; uint8_t c[4]; } u;
+	u.c[1] = b[0];
+	u.c[2] = b[1];
+	u.c[3] = b[2];
+	return u.x >> 8;
+}
+
+
+void iv::perception::GPSSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
+{
+    int i = strlen(str), j = 0, counter = 0;
+    char c[2];
+    unsigned int bytes[2];
+
+    for (j = 0; j < i; j += 2)
+    {
+        if(0 == j % 2)
+        {
+            c[0] = str[j];
+            c[1] = str[j + 1];
+            sscanf(c, "%02x" , &bytes[0]);
+            UnChar[counter] = bytes[0];
+            counter++;
+        }
+    }
+}
+
+void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
+{
+    iv::GPSData data(new iv::GPS_INS);
+    data->ins_status = xgpsimu.ins_state();
+    data->rtk_status = xgpsimu.rtk_state();
+
+    data->accel_x = xgpsimu.acce_x();
+    data->accel_y = xgpsimu.acce_y();
+    data->accel_z = xgpsimu.acce_z();
+    data->ang_rate_x = xgpsimu.gyro_x();
+    data->ang_rate_y = xgpsimu.gyro_y();
+    data->ang_rate_z = xgpsimu.gyro_z();
+
+    data->gps_lat = xgpsimu.lat();
+    data->gps_lng = xgpsimu.lon();
+    data->gps_z = xgpsimu.height();
+
+    data->ins_heading_angle = xgpsimu.heading();
+    data->ins_pitch_angle = xgpsimu.pitch();
+    data->ins_roll_angle = xgpsimu.roll();
+
+    data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
+
+    GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+
+    ServiceCarStatus.mRTKStatus = data->rtk_status;
+
+    ServiceCarStatus.mfGPSAcc = xgpsimu.acc_calc();
+
+    ServiceCarStatus.speed = data->speed;
+//       qDebug("speed x is %g",ServiceCarStatus.speed);
+    if(data->ins_status == 4)
+        signal_gps_data->operator()(data);	//传输数据
+
+    ServiceCarStatus.location->gps_lat = data->gps_lat;
+    ServiceCarStatus.location->gps_lng = data->gps_lng;
+    ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
+    ServiceCarStatus.location->gps_x = data->gps_x;
+    ServiceCarStatus.location->gps_y = data->gps_y;
+    ServiceCarStatus.location->rtk_status = data->rtk_status;
+    ServiceCarStatus.location->ins_status = data->ins_status;
+
+}

src/decition/common/perception_sf/impl_lidar.cpp

@@ -1,157 +1,157 @@
-/*
-* 在此方法中处理激光雷达的打开、处理数据、关闭等
-* 识别到障碍物时 触发signal_lidar_obstacle
-*
-*/
-#include <perception_sf/sensor_lidar.h>
-#include <QDebug>
-
-#include "perceptionoutput.h"
-
-std::mutex mtx_nomal;
-
-
-
-void iv::perception::LiDARSensor::lidarcollect()
-{
-
-}
-
-void iv::perception::LiDARSensor::processSensor()
-{
-
-}
-
-
-void iv::perception::LiDARSensor::UpdatePer(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > lidar_per)
-{
-//    std::cout<<"update lidar"<<std::endl;
-    ServiceLidar.copyfromlidarper(lidar_per);
-//    ServiceLidar.copylidarperto(lidar_per);
-}
-
-// txs 12.16
-void iv::perception::LiDARSensor::updataFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)
-{
-    mMutex.lock();
-    fusion_obs.swap(mfusion_obs_);
-    mMutex.unlock();
-
-
-
-    fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
-    memset(fusion_ptr_,0,sizeof(Obs_grid[iv::grx][iv::gry]));
-    for(int i =0; i<iv::grx; i++)     //复制到指针数组
-    {
-        for(int j =0; j<iv::gry; j++)
-        {
-            fusion_ptr_[i*(iv::gry)+j].ob = 0;
-        }
-
-    }
-
-    for(int i =0; i<mfusion_obs_->obj_size();i++)
-    {
-        iv::fusion::fusionobject xobs = mfusion_obs_->obj(i);
-        for(int j =0; j<xobs.nomal_centroid().size(); j++)
-        {
-            int dx, dy;
-            dx = (xobs.nomal_centroid(j).nomal_x() + gridwide*(double)centerx)/gridwide;
-            dy = (xobs.nomal_centroid(j).nomal_y() + gridwide*(double)centery)/gridwide;
-
-
-            if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
-            {
-                fusion_ptr_[dx*(iv::gry) + dy].id = xobs.id();
-                fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
-                fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
-                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().x();
-                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().y();
-                fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
-                fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
-                fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
-            }
-        }
-
-    }
- //   signal_fusion_obstacle_grid->operator()(fusion_ptr_);
-
-
-
-
-
-
-    signal_lidar_obstacle_grid->operator()(fusion_ptr_);//触发
-
-}
-// txs 12.16
-
-
-
-void iv::perception::LiDARSensor::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
-{
-    mMutex.lock();
-    lidar_obs.swap(mobs);
-    mMutex.unlock();
-
-    ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
-    memset(ptr, 0, sizeof(Obs_grid[iv::grx][iv::gry]));
-    for (int i = 0; i <iv::grx; i++)//复制到指针数组
-    {
-        for (int j = 0; j <iv::gry; j++)
-        {
-//            ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
-//            ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
-            ptr[i * (iv::gry) + j].ob = 0;
-//            ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
-//            ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
-        }
-    }
-    for(int i=0;i<mobs->size();i++)
-    {
-        iv::ObstacleBasic xobs = mobs->at(i);
-        int dx,dy;
-        dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
-        dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
-        if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
-        {
-            ptr[dx*(iv::gry) +dy].high = xobs.high;
-            ptr[dx*(iv::gry) +dy].low = xobs.low;
-            ptr[dx*(iv::gry) + dy].ob = 2;
-            ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
-            ptr[dx*(iv::gry) + dy].pointcount = 5;
-        }
-
-    }
-
-//    std::cout<<"update obs."<<std::endl;
-    signal_lidar_obstacle_grid->operator()(ptr);//触发
-
-    ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-    int i;
-    int nSize = mobs->size();
-    for(i=0;i<nSize;i++)
-        _obs_grid_->push_back(mobs->at(i));
-    ServiceLidar.copyfromlidarobs(_obs_grid_);   
-
-    ServiceCarStatus.mLidarS = 10;
-}
-
-void iv::perception::LiDARSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
-{
-    int i = strlen(str), j = 0, counter = 0;
-    char c[2];
-    unsigned int bytes[2];
-
-    for (j = 0; j < i; j += 2)
-    {
-        if(0 == j % 2)
-        {
-            c[0] = str[j];
-            c[1] = str[j + 1];
-            sscanf(c, "%02x" , &bytes[0]);
-            UnChar[counter] = bytes[0];
-            counter++;
-        }
-    }
-}
+/*
+* 在此方法中处理激光雷达的打开、处理数据、关闭等
+* 识别到障碍物时 触发signal_lidar_obstacle
+*
+*/
+#include <perception_sf/sensor_lidar.h>
+#include <QDebug>
+
+#include "perceptionoutput.h"
+
+std::mutex mtx_nomal;
+
+
+
+void iv::perception::LiDARSensor::lidarcollect()
+{
+
+}
+
+void iv::perception::LiDARSensor::processSensor()
+{
+
+}
+
+
+void iv::perception::LiDARSensor::UpdatePer(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput> > lidar_per)
+{
+//    std::cout<<"update lidar"<<std::endl;
+    ServiceLidar.copyfromlidarper(lidar_per);
+//    ServiceLidar.copylidarperto(lidar_per);
+}
+
+// txs 12.16
+void iv::perception::LiDARSensor::updataFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)
+{
+    mMutex.lock();
+    fusion_obs.swap(mfusion_obs_);
+    mMutex.unlock();
+
+
+
+    fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+    memset(fusion_ptr_,0,sizeof(Obs_grid[iv::grx][iv::gry]));
+    for(int i =0; i<iv::grx; i++)     //复制到指针数组
+    {
+        for(int j =0; j<iv::gry; j++)
+        {
+            fusion_ptr_[i*(iv::gry)+j].ob = 0;
+        }
+
+    }
+
+    for(int i =0; i<mfusion_obs_->obj_size();i++)
+    {
+        iv::fusion::fusionobject xobs = mfusion_obs_->obj(i);
+        for(int j =0; j<xobs.nomal_centroid().size(); j++)
+        {
+            int dx, dy;
+            dx = (xobs.nomal_centroid(j).nomal_x() + gridwide*(double)centerx)/gridwide;
+            dy = (xobs.nomal_centroid(j).nomal_y() + gridwide*(double)centery)/gridwide;
+
+
+            if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
+            {
+                fusion_ptr_[dx*(iv::gry) + dy].id = xobs.id();
+                fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
+                fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().x();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().y();
+                fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
+                fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
+                fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
+            }
+        }
+
+    }
+ //   signal_fusion_obstacle_grid->operator()(fusion_ptr_);
+
+
+
+
+
+
+    signal_lidar_obstacle_grid->operator()(fusion_ptr_);//触发
+
+}
+// txs 12.16
+
+
+
+void iv::perception::LiDARSensor::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
+{
+    mMutex.lock();
+    lidar_obs.swap(mobs);
+    mMutex.unlock();
+
+    ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+    memset(ptr, 0, sizeof(Obs_grid[iv::grx][iv::gry]));
+    for (int i = 0; i <iv::grx; i++)//复制到指针数组
+    {
+        for (int j = 0; j <iv::gry; j++)
+        {
+//            ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
+//            ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
+            ptr[i * (iv::gry) + j].ob = 0;
+//            ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
+//            ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
+        }
+    }
+    for(int i=0;i<mobs->size();i++)
+    {
+        iv::ObstacleBasic xobs = mobs->at(i);
+        int dx,dy;
+        dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
+        dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
+        if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
+        {
+            ptr[dx*(iv::gry) +dy].high = xobs.high;
+            ptr[dx*(iv::gry) +dy].low = xobs.low;
+            ptr[dx*(iv::gry) + dy].ob = 2;
+            ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
+            ptr[dx*(iv::gry) + dy].pointcount = 5;
+        }
+
+    }
+
+//    std::cout<<"update obs."<<std::endl;
+    signal_lidar_obstacle_grid->operator()(ptr);//触发
+
+    ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+    int i;
+    int nSize = mobs->size();
+    for(i=0;i<nSize;i++)
+        _obs_grid_->push_back(mobs->at(i));
+    ServiceLidar.copyfromlidarobs(_obs_grid_);   
+
+    ServiceCarStatus.mLidarS = 10;
+}
+
+void iv::perception::LiDARSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
+{
+    int i = strlen(str), j = 0, counter = 0;
+    char c[2];
+    unsigned int bytes[2];
+
+    for (j = 0; j < i; j += 2)
+    {
+        if(0 == j % 2)
+        {
+            c[0] = str[j];
+            c[1] = str[j + 1];
+            sscanf(c, "%02x" , &bytes[0]);
+            UnChar[counter] = bytes[0];
+            counter++;
+        }
+    }
+}

src/decition/common/perception_sf/impl_radar.cpp

@@ -1,32 +1,32 @@
-/*
-* 在此方法中处理毫米波雷达的打开、处理数据、关闭等
-* 识别到障碍物时 触发signal_radar_obstacle
-*
-*/
-
-#include <perception_sf/sensor_radar.h>
-#include <control/can.h>
-void iv::perception::RadarSensor::processSensor()
-{
-
-}
-
-void iv::perception::RadarSensor::UpdateRADAR(iv::radar::radarobjectarray xarray)
-{
-
-    int i;
-    for(i=0;i<RADAR_OBJ_NUM;i++)
-    {
-        ServiceCarStatus.obs_radar[i].valid = false;
-    }
-
-    for(i=0;i<xarray.obj_size();i++)
-    {
-        ServiceCarStatus.obs_radar[i].valid = xarray.obj(i).bvalid();
-        ServiceCarStatus.obs_radar[i].nomal_x = xarray.obj(i).x();
-        ServiceCarStatus.obs_radar[i].nomal_y = xarray.obj(i).y();
-        ServiceCarStatus.obs_radar[i].speed_relative = xarray.obj(i).vel();
-        ServiceCarStatus.obs_radar[i].speed_x = xarray.obj(i).vx();
-        ServiceCarStatus.obs_radar[i].speed_y = xarray.obj(i).vy();
-    }
-}
+/*
+* 在此方法中处理毫米波雷达的打开、处理数据、关闭等
+* 识别到障碍物时 触发signal_radar_obstacle
+*
+*/
+
+#include <perception_sf/sensor_radar.h>
+#include <control/can.h>
+void iv::perception::RadarSensor::processSensor()
+{
+
+}
+
+void iv::perception::RadarSensor::UpdateRADAR(iv::radar::radarobjectarray xarray)
+{
+
+    int i;
+    for(i=0;i<RADAR_OBJ_NUM;i++)
+    {
+        ServiceCarStatus.obs_radar[i].valid = false;
+    }
+
+    for(i=0;i<xarray.obj_size();i++)
+    {
+        ServiceCarStatus.obs_radar[i].valid = xarray.obj(i).bvalid();
+        ServiceCarStatus.obs_radar[i].nomal_x = xarray.obj(i).x();
+        ServiceCarStatus.obs_radar[i].nomal_y = xarray.obj(i).y();
+        ServiceCarStatus.obs_radar[i].speed_relative = xarray.obj(i).vel();
+        ServiceCarStatus.obs_radar[i].speed_x = xarray.obj(i).vx();
+        ServiceCarStatus.obs_radar[i].speed_y = xarray.obj(i).vy();
+    }
+}

src/decition/common/perception_sf/sensor.h

@@ -1,90 +1,90 @@
-/**
-*传感器接口
-*/
-#ifndef _IV_PERCEPTION_SENSOR_
-#define _IV_PERCEPTION_SENSOR_
-
-#include <common_sf/boost.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <string.h>
-#include <iostream>
-#include <sys/types.h>
-#include <common_sf/obstacle_type.h>
-#include <QtNetwork/QTcpSocket>
-#include <QtNetwork/QUdpSocket>
-
-namespace iv {
-	namespace perception {
-		class Sensor
-		{
-		public:
-			Sensor() {};
-			~Sensor() {};
-
-
-            virtual void start() = 0;/*开始感知环境*/
-
-
-            virtual void stop() = 0;/* 停止感知*/
-
-
-            virtual bool isRunning() const = 0;/* 判断是否运行*/
-
-
-            virtual void processSensor() = 0;/* 传感器执行*/
-
-
-            template<typename T>//注册回调函数
-			boost::signals2::connection registerCallBack(const boost::function<T> &callback);
-			//template<typename T>
-			//boost::signals2::connection registerCallBack2(const boost::function<T> &callback);
-
-
-            template<typename T>//建立signal
-			boost::signals2::signal<T>* createSignal();
-			//template<typename T>
-			//boost::signals2::signal<T>* createSignal2();
-
-		protected:
-			boost::signals2::signal_base* signal_base;	//信号基类  存储传感器建立的信号
-			//boost::signals2::signal_base* signal_base2;	//信号基类  存储传感器建立的信号
-			boost::thread* thread_sensor_run_;			//传感器运行的线程
-		};
-
-
-
-
-        template<typename T> boost::signals2::connection// 注册回调函数
-		Sensor::registerCallBack(const boost::function<T> &callback) {
-			boost::signals2::signal<T>* signal = dynamic_cast<boost::signals2::signal<T>*>(signal_base);
-			boost::signals2::connection ret = signal->connect(callback);
-			return (ret);
-		}
-		//template<typename T> boost::signals2::connection
-		//	Sensor::registerCallBack2(const boost::function<T> &callback) {
-		//	boost::signals2::signal<T>* signal = dynamic_cast<boost::signals2::signal<T>*>(signal_base2);
-		//	boost::signals2::connection ret = signal->connect(callback);
-		//	return (ret);
-		//}
-
-		//建立signal
-		template<typename T>
-		boost::signals2::signal<T>* Sensor::createSignal() {
-			boost::signals2::signal<T>* signal = new boost::signals2::signal<T>();
-			signal_base = signal;
-			return signal;
-		}
-		//template<typename T>
-		//boost::signals2::signal<T>* Sensor::createSignal2() {
-		//	boost::signals2::signal<T>* signal = new boost::signals2::signal<T>();
-		//	signal_base2 = signal;
-		//	return signal;
-		//}
-
-	}
-}
-
-#endif 
+/**
+*传感器接口
+*/
+#ifndef _IV_PERCEPTION_SENSOR_
+#define _IV_PERCEPTION_SENSOR_
+
+#include <common_sf/boost.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <iostream>
+#include <sys/types.h>
+#include <common_sf/obstacle_type.h>
+#include <QtNetwork/QTcpSocket>
+#include <QtNetwork/QUdpSocket>
+
+namespace iv {
+	namespace perception {
+		class Sensor
+		{
+		public:
+			Sensor() {};
+			~Sensor() {};
+
+
+            virtual void start() = 0;/*开始感知环境*/
+
+
+            virtual void stop() = 0;/* 停止感知*/
+
+
+            virtual bool isRunning() const = 0;/* 判断是否运行*/
+
+
+            virtual void processSensor() = 0;/* 传感器执行*/
+
+
+            template<typename T>//注册回调函数
+			boost::signals2::connection registerCallBack(const boost::function<T> &callback);
+			//template<typename T>
+			//boost::signals2::connection registerCallBack2(const boost::function<T> &callback);
+
+
+            template<typename T>//建立signal
+			boost::signals2::signal<T>* createSignal();
+			//template<typename T>
+			//boost::signals2::signal<T>* createSignal2();
+
+		protected:
+			boost::signals2::signal_base* signal_base;	//信号基类  存储传感器建立的信号
+			//boost::signals2::signal_base* signal_base2;	//信号基类  存储传感器建立的信号
+			boost::thread* thread_sensor_run_;			//传感器运行的线程
+		};
+
+
+
+
+        template<typename T> boost::signals2::connection// 注册回调函数
+		Sensor::registerCallBack(const boost::function<T> &callback) {
+			boost::signals2::signal<T>* signal = dynamic_cast<boost::signals2::signal<T>*>(signal_base);
+			boost::signals2::connection ret = signal->connect(callback);
+			return (ret);
+		}
+		//template<typename T> boost::signals2::connection
+		//	Sensor::registerCallBack2(const boost::function<T> &callback) {
+		//	boost::signals2::signal<T>* signal = dynamic_cast<boost::signals2::signal<T>*>(signal_base2);
+		//	boost::signals2::connection ret = signal->connect(callback);
+		//	return (ret);
+		//}
+
+		//建立signal
+		template<typename T>
+		boost::signals2::signal<T>* Sensor::createSignal() {
+			boost::signals2::signal<T>* signal = new boost::signals2::signal<T>();
+			signal_base = signal;
+			return signal;
+		}
+		//template<typename T>
+		//boost::signals2::signal<T>* Sensor::createSignal2() {
+		//	boost::signals2::signal<T>* signal = new boost::signals2::signal<T>();
+		//	signal_base2 = signal;
+		//	return signal;
+		//}
+
+	}
+}
+
+#endif 

src/decition/common/perception_sf/sensor_camera.cpp

@@ -1,23 +1,23 @@
-#include <perception_sf/sensor_camera.h>
-
-iv::perception::CameraSensor::CameraSensor() {
-	signal_camera_obstacle = createSignal<sig_cb_camera_sensor_obstacle>();
-}
-
-void iv::perception::CameraSensor::start()
-{
-	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::CameraSensor::processSensor, this));
-	std::cout << "camera thread Run" << std::endl;
-}
-
-void iv::perception::CameraSensor::stop()
-{
-	thread_sensor_run_->interrupt();
-	thread_sensor_run_->join();
-	std::cout << "camera thread Stop" << std::endl;
-}
-
-bool iv::perception::CameraSensor::isRunning() const
-{
-	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
-}
+#include <perception_sf/sensor_camera.h>
+
+iv::perception::CameraSensor::CameraSensor() {
+	signal_camera_obstacle = createSignal<sig_cb_camera_sensor_obstacle>();
+}
+
+void iv::perception::CameraSensor::start()
+{
+	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::CameraSensor::processSensor, this));
+	std::cout << "camera thread Run" << std::endl;
+}
+
+void iv::perception::CameraSensor::stop()
+{
+	thread_sensor_run_->interrupt();
+	thread_sensor_run_->join();
+	std::cout << "camera thread Stop" << std::endl;
+}
+
+bool iv::perception::CameraSensor::isRunning() const
+{
+	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
+}

src/decition/common/perception_sf/sensor_camera.h

@@ -1,33 +1,33 @@
-#pragma once
-#ifndef _IV_PERCEPTION_SENSOR_CAMERA_
-#define _IV_PERCEPTION_SENSOR_CAMERA_
-
-#include <perception_sf/sensor.h>
-namespace iv {
-	namespace perception {
-		class CameraSensor : public iv::perception::Sensor
-		{
-		public:
-			CameraSensor();
-			~CameraSensor();
-
-			typedef void
-			(sig_cb_camera_sensor_obstacle)(iv::ObsCamera);
-
-			// 通过 Sensor 继承
-			virtual void start() override;
-
-			virtual void stop() override;
-
-			virtual bool isRunning() const override;
-
-			virtual void processSensor() override;
-
-		private:
-			boost::signals2::signal<sig_cb_camera_sensor_obstacle>* signal_camera_obstacle;	//摄像头障碍物信号
-
-		};
-	}
-}
-
-#endif // !_IV_PERCEPTION_SENSOR_CAMERA_
+#pragma once
+#ifndef _IV_PERCEPTION_SENSOR_CAMERA_
+#define _IV_PERCEPTION_SENSOR_CAMERA_
+
+#include <perception_sf/sensor.h>
+namespace iv {
+	namespace perception {
+		class CameraSensor : public iv::perception::Sensor
+		{
+		public:
+			CameraSensor();
+			~CameraSensor();
+
+			typedef void
+			(sig_cb_camera_sensor_obstacle)(iv::ObsCamera);
+
+			// 通过 Sensor 继承
+			virtual void start() override;
+
+			virtual void stop() override;
+
+			virtual bool isRunning() const override;
+
+			virtual void processSensor() override;
+
+		private:
+			boost::signals2::signal<sig_cb_camera_sensor_obstacle>* signal_camera_obstacle;	//摄像头障碍物信号
+
+		};
+	}
+}
+
+#endif // !_IV_PERCEPTION_SENSOR_CAMERA_

src/decition/common/perception_sf/sensor_gps.cpp

@@ -1,63 +1,63 @@
-#include <perception_sf/sensor_gps.h>
-#include "ivlog.h"
-
-extern std::string gstrmemgps;
-extern iv::Ivlog * givlog;
-
-namespace iv {
-namespace perception {
-    GPSSensor * ggpsimu;
-
-    void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-    {
-//        ggpsimu->UpdateGPSIMU(strdata,nSize);
-
-        iv::gps::gpsimu xgpsimu;
-        if(!xgpsimu.ParseFromArray(strdata,nSize))
-        {
-            std::cout<<"ListenRaw Parse error."<<std::endl;
-        }
-//        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());
-        ggpsimu->UpdateGPSIMU(xgpsimu);
-
-        static qint64 oldtime;
-        if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
-        {
-            qDebug("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
-            givlog->warn("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
-        }
-        givlog->verbose("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
-        oldtime = QDateTime::currentMSecsSinceEpoch();
-
-    }
-}
-}
-
-iv::perception::GPSSensor::GPSSensor() {
-	signal_gps_data = createSignal<sig_cb_gps_sensor_data>();
-    ggpsimu = this;
-}
-iv::perception::GPSSensor::~GPSSensor() {
-
-}
-
-void iv::perception::GPSSensor::start()
-{
-    thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::GPSSensor::processSensor, this));
-
-    mpagpsimu = iv::modulecomm::RegisterRecv(gstrmemgps.data(),iv::perception::Listengpsimu);
-    //OutputDebugString(TEXT("\nGPS thread Run\n"));
-}
-
-void iv::perception::GPSSensor::stop()
-{
-	thread_sensor_run_->interrupt();
-	thread_sensor_run_->join();
-    //OutputDebugString(TEXT("\nGPS thread Stop\n"));
-}
-
-bool iv::perception::GPSSensor::isRunning() const
-{
-	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
-}
-
+#include <perception_sf/sensor_gps.h>
+#include "ivlog.h"
+
+extern std::string gstrmemgps;
+extern iv::Ivlog * givlog;
+
+namespace iv {
+namespace perception {
+    GPSSensor * ggpsimu;
+
+    void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+    {
+//        ggpsimu->UpdateGPSIMU(strdata,nSize);
+
+        iv::gps::gpsimu xgpsimu;
+        if(!xgpsimu.ParseFromArray(strdata,nSize))
+        {
+            std::cout<<"ListenRaw Parse error."<<std::endl;
+        }
+//        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());
+        ggpsimu->UpdateGPSIMU(xgpsimu);
+
+        static qint64 oldtime;
+        if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
+        {
+            qDebug("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+            givlog->warn("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+        }
+        givlog->verbose("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+        oldtime = QDateTime::currentMSecsSinceEpoch();
+
+    }
+}
+}
+
+iv::perception::GPSSensor::GPSSensor() {
+	signal_gps_data = createSignal<sig_cb_gps_sensor_data>();
+    ggpsimu = this;
+}
+iv::perception::GPSSensor::~GPSSensor() {
+
+}
+
+void iv::perception::GPSSensor::start()
+{
+    thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::GPSSensor::processSensor, this));
+
+    mpagpsimu = iv::modulecomm::RegisterRecv(gstrmemgps.data(),iv::perception::Listengpsimu);
+    //OutputDebugString(TEXT("\nGPS thread Run\n"));
+}
+
+void iv::perception::GPSSensor::stop()
+{
+	thread_sensor_run_->interrupt();
+	thread_sensor_run_->join();
+    //OutputDebugString(TEXT("\nGPS thread Stop\n"));
+}
+
+bool iv::perception::GPSSensor::isRunning() const
+{
+	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
+}
+

src/decition/common/perception_sf/sensor_gps.h

@@ -1,222 +1,222 @@
-#pragma once
-#ifndef _IV_PERCEPTION_SENSOR_GPS_
-#define _IV_PERCEPTION_SENSOR_GPS_
-
-#include <perception_sf/sensor.h>
-#include <common_sf/gps_type.h>
-#include <common_sf/boost.h>
-
-#include "modulecomm.h"
-#include "gpsimu.pb.h"
-
-// Definitions.
-
-// General constants.
-#define NOUTPUT_PACKET_LENGTH  (72)       //!< NCom packet length.
-#define NCOM_SYNC           (0xE7)        //!< NCom sync byte.
-#define PKT_PERIOD          (0.01)        //!< 10ms updates.
-#define TIME2SEC            (1e-3)        //!< Units of 1 ms.
-#define FINETIME2SEC        (4e-6)        //!< Units of 4 us.
-#define TIMECYCLE           (60000)       //!< Units of TIME2SEC (i.e. 60 seconds).
-#define WEEK2CYCLES         (10080)       //!< Time cycles in a week.
-#define ACC2MPS2            (1e-4)        //!< Units of 0.1 mm/s^2.
-#define RATE2RPS            (1e-5)        //!< Units of 0.01 mrad/s.
-#define VEL2MPS             (1e-4)        //!< Units of 0.1 mm/s.
-#define ANG2RAD             (1e-6)        //!< Units of 0.001 mrad.
-#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
-#define POSA2M              (1e-3)        //!< Units of 1 mm.
-#define VELA2MPS            (1e-3)        //!< Units of 1 mm/s.
-#define ANGA2RAD            (1e-5)        //!< Units of 0.01 mrad.
-#define GB2RPS              (5e-6)        //!< Units of 0.005 mrad/s.
-#define AB2MPS2             (1e-4)        //!< Units of 0.1 mm/s^2.
-#define GSFACTOR            (1e-6)        //!< Units of 1 ppm.
-#define ASFACTOR            (1e-6)        //!< Units of 1 ppm.
-#define GBA2RPS             (1e-6)        //!< Units of 0.001 mrad/s.
-#define ABA2MPS2            (1e-5)        //!< Units of 0.01 mm/s^2.
-#define GSAFACTOR           (1e-6)        //!< Units of 1 ppm.
-#define ASAFACTOR           (1e-6)        //!< Units of 1 ppm.
-#define GPSPOS2M            (1e-3)        //!< Units of 1 mm.
-#define GPSATT2RAD          (1e-4)        //!< Units of 0.1 mrad.
-#define GPSPOSA2M           (1e-4)        //!< Units of 0.1 mm.
-#define GPSATTA2RAD         (1e-5)        //!< Units of 0.01 mrad.
-#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
-#define DIFFAGE2SEC         (1e-2)        //!< Units of 0.01 s.
-#define REFPOS2M            (0.0012)      //!< Units of 1.2 mm.
-#define REFANG2RAD          (1e-4)        //!< Units of 0.1 mrad.
-#define OUTPOS2M            (1e-3)        //!< Units of 1 mm.
-#define ZVPOS2M             (1e-3)        //!< Units of 1 mm.
-#define ZVPOSA2M            (1e-4)        //!< Units of 0.1 mm.
-#define NSPOS2M             (1e-3)        //!< Units of 1 mm.
-#define NSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
-#define ALIGN2RAD           (1e-4)        //!< Units of 0.1 mrad.
-#define ALIGNA2RAD          (1e-5)        //!< Units of 0.01 mrad.
-#define SZVDELAY2S          (1.0)         //!< Units of 1.0 s.
-#define SZVPERIOD2S         (0.1)         //!< Units of 0.1 s.
-#define TOPSPEED2MPS        (0.5)         //!< Units of 0.5 m/s.
-#define NSDELAY2S           (0.1)         //!< Units of 0.1 s.
-#define NSPERIOD2S          (0.02)        //!< Units of 0.02 s.
-#define NSACCEL2MPS2        (0.04)        //!< Units of 0.04 m/s^2.
-#define NSSPEED2MPS         (0.1)         //!< Units of 0.1 m/s.
-#define NSRADIUS2M          (0.5)         //!< Units of 0.5 m.
-#define INITSPEED2MPS       (0.1)         //!< Units of 0.1 m/s.
-#define HLDELAY2S           (1.0)         //!< Units of 1.0 s.
-#define HLPERIOD2S          (0.1)         //!< Units of 0.1 s.
-#define STATDELAY2S         (1.0)         //!< Units of 1.0 s.
-#define STATSPEED2MPS       (0.01)        //!< Units of 1.0 cm/s.
-#define WSPOS2M             (1e-3)        //!< Units of 1 mm.
-#define WSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
-#define WSSF2PPM            (0.1)         //!< Units of 0.1 pulse per metre (ppm).
-#define WSSFA2PC            (0.002)       //!< Units of 0.002% of scale factor.
-#define WSDELAY2S           (0.1)         //!< Units of 0.1 s.
-#define WSNOISE2CNT         (0.1)         //!< Units of 0.1 count for wheel speed noise.
-#define UNDUL2M             (0.005)       //!< Units of 5 mm.
-#define DOPFACTOR           (0.1)         //!< Resolution of 0.1.
-#define OMNISTAR_MIN_FREQ   (1.52e9)      //!< (Hz) i.e. 1520.0 MHz.
-#define OMNIFREQ2HZ         (1000.0)      //!< Resolution of 1 kHz.
-#define SNR2DB              (0.2)         //!< Resolution of 0.2 dB.
-#define LTIME2SEC           (1.0)         //!< Resolution of 1.0 s.
-#define TEMPK_OFFSET        (203.15)      //!< Temperature offset in degrees K.
-#define ABSZERO_TEMPC       (-273.15)     //!< Absolute zero (i.e. 0 deg K) in deg C.
-
-// For more accurate and complete local coordinates
-#define FINEANG2RAD (1.74532925199433e-9) //!< Units of 0.1 udeg.
-#define ALT2M               (1e-3)        //!< Units of 1 mm.
-
-// For GPS supply voltage
-#define SUPPLYV2V           (0.1)         //!< Units of 0.1 V.
-
-// Mathematical constant definitions
-#ifndef M_PI
-#define M_PI (3.1415926535897932384626433832795)  //!< Pi.
-#endif
-#define DEG2RAD             (M_PI/180.0)  //!< Convert degrees to radians.
-#define RAD2DEG             (180.0/M_PI)  //!< Convert radians to degrees.
-#define POS_INT_24          (8388607)     //!< Maximum value of a two's complement 24 bit integer.
-#define NEG_INT_24          (-8388607)    //!< Minimum value of a two's complement 24 bit integer.
-#define INV_INT_24          (-8388608)    //!< Represents an invalid two's complement 24 bit integer.
-
-#define NCOM_COUNT_TOO_OLD  (150)         //!< Cycle counter for data too old.
-#define NCOM_STDCNT_MAX     (0xFF)        //!< Definition for the RTBNS accuracy counter.
-#define MIN_HORZ_SPEED      (0.07)        //!< 0.07 m/s hold distance.
-#define MIN_VERT_SPEED      (0.07)        //!< 0.07 m/s hold distance.
-#define SPEED_HOLD_FACTOR   (2.0)         //!< Hold distance when speed within 2 sigma of 0.
-#define MINUTES_IN_WEEK     (10080)       //!< Number of minutes in a week.
-
-// OmniStar status definitions
-#define NCOM_OMNI_STATUS_UNKNOWN      (0xFF)
-#define NCOM_OMNI_STATUS_VBSEXPIRED   (0x01)
-#define NCOM_OMNI_STATUS_VBSREGION    (0x02)
-#define NCOM_OMNI_STATUS_VBSNOBASE    (0x04)
-#define NCOM_OMNI_STATUS_HPEXPIRED    (0x08)
-#define NCOM_OMNI_STATUS_HPREGION     (0x10)
-#define NCOM_OMNI_STATUS_HPNOBASE     (0x20)
-#define NCOM_OMNI_STATUS_HPNOCONVERGE (0x40)
-#define NCOM_OMNI_STATUS_HPKEYINVALID (0x80)
-
-// GPS hardware status definitions
-#define NCOM_GPS_ANT_STATUS_BITMASK   (0x03)
-#define NCOM_GPS_ANT_STATUS_DONTKNOW  (0x03)
-#define NCOM_GPS_ANT_STATUS_BITSHIFT  (0)
-#define NCOM_GPS_ANT_POWER_BITMASK    (0x0C)
-#define NCOM_GPS_ANT_POWER_DONTKNOW   (0x0C)
-#define NCOM_GPS_ANT_POWER_BITSHIFT   (2)
-
-// GPS feature set 1 definitions
-#define NCOM_GPS_FEATURE_PSRDIFF      (0x01)
-#define NCOM_GPS_FEATURE_SBAS         (0x02)
-#define NCOM_GPS_FEATURE_OMNIVBS      (0x08)
-#define NCOM_GPS_FEATURE_OMNIHP       (0x10)
-#define NCOM_GPS_FEATURE_L1DIFF       (0x20)
-#define NCOM_GPS_FEATURE_L1L2DIFF     (0x40)
-
-// GPS feature set 2 definitions
-#define NCOM_GPS_FEATURE_GLONASS      (0x01)
-#define NCOM_GPS_FEATURE_GALILEO      (0x02)
-#define NCOM_GPS_FEATURE_RAWRNG       (0x04)
-#define NCOM_GPS_FEATURE_RAWDOP       (0x08)
-#define NCOM_GPS_FEATURE_RAWL1        (0x10)
-#define NCOM_GPS_FEATURE_RAWL2        (0x20)
-#define NCOM_GPS_FEATURE_RAWL5        (0x40)
-
-// GPS feature valid definition
-#define NCOM_GPS_FEATURE_VALID        (0x80)
-
-// The start of GPS time in a time_t style. In this version it is a constant, but this constant assumes that
-// the local machine uses 00:00:00 01/01/1970 as its Epoch time. If your machine is different then you need to
-// convert 00:00:00 06/01/1980 in to the local machine's time_t time.
-#define GPS_TIME_START_TIME_T         (315964800)
-
-// Second order filter class
-#define INPUT_JITTER_TOLERANCE     (0.01)  // i.e. 1%
-
-
-/*index*/
-#define		PI_SYNC             0
-#define		PI_TIME             1
-#define		PI_ACCEL_X          3
-#define		PI_ACCEL_Y          6
-#define		PI_ACCEL_Z          9
-#define		PI_ANG_RATE_X      12
-#define		PI_ANG_RATE_Y      15
-#define		PI_ANG_RATE_Z      18
-#define		PI_INS_NAV_MODE    21
-#define		PI_CHECKSUM_1      22
-#define		PI_POS_LAT         23
-#define		PI_POS_LON         31
-#define		PI_POS_ALT         39
-#define		PI_VEL_N           43
-#define		PI_VEL_E           46
-#define		PI_VEL_D           49
-#define		PI_ORIEN_H         52
-#define		PI_ORIEN_P         55
-#define		PI_ORIEN_R         58
-#define		PI_CHECKSUM_2      61
-#define		PI_CHANNEL_INDEX   62
-#define		PI_CHANNEL_STATUS  63
-#define		PI_SAT_NUM		   67
-#define		PI_RTK_STATUS	   68
-#define		PI_CHECKSUM_3      71
-
-/*RTK IMU status check*/
-#define		RTK_IMU_OK		0
-#define		IMU_STATUS_ERR	1
-#define		RTK_STATUS_ERR	2
-#define		UNKNOWN			0xFF
-
-namespace iv {
-	namespace perception {
-		class GPSSensor : public iv::perception::Sensor
-		{
-		public:
-			GPSSensor();
-			~GPSSensor();
-
-			// 通过 Sensor 继承
-			virtual void start() override;
-
-			virtual void stop() override;
-
-			virtual bool isRunning() const override;
-
-			virtual void processSensor() override;
-            void convertStrToUnChar(char* str, unsigned char* UnChar);
-
-			/* gps 惯导 数据*/
-			typedef void
-			(sig_cb_gps_sensor_data)(iv::GPSData);
-
-		private:
-			boost::signals2::signal<sig_cb_gps_sensor_data>* signal_gps_data;	//GPS 惯导数据信号
-
-        private:
-            void * mpagpsimu;
-
-        public:
-            void UpdateGPSIMU(iv::gps::gpsimu xgpsimu);
-		};
-
-
-	}
-}
-
-#endif // !_IV_PERCEPTION_SENSOR_GPS_
+#pragma once
+#ifndef _IV_PERCEPTION_SENSOR_GPS_
+#define _IV_PERCEPTION_SENSOR_GPS_
+
+#include <perception_sf/sensor.h>
+#include <common_sf/gps_type.h>
+#include <common_sf/boost.h>
+
+#include "modulecomm.h"
+#include "gpsimu.pb.h"
+
+// Definitions.
+
+// General constants.
+#define NOUTPUT_PACKET_LENGTH  (72)       //!< NCom packet length.
+#define NCOM_SYNC           (0xE7)        //!< NCom sync byte.
+#define PKT_PERIOD          (0.01)        //!< 10ms updates.
+#define TIME2SEC            (1e-3)        //!< Units of 1 ms.
+#define FINETIME2SEC        (4e-6)        //!< Units of 4 us.
+#define TIMECYCLE           (60000)       //!< Units of TIME2SEC (i.e. 60 seconds).
+#define WEEK2CYCLES         (10080)       //!< Time cycles in a week.
+#define ACC2MPS2            (1e-4)        //!< Units of 0.1 mm/s^2.
+#define RATE2RPS            (1e-5)        //!< Units of 0.01 mrad/s.
+#define VEL2MPS             (1e-4)        //!< Units of 0.1 mm/s.
+#define ANG2RAD             (1e-6)        //!< Units of 0.001 mrad.
+#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
+#define POSA2M              (1e-3)        //!< Units of 1 mm.
+#define VELA2MPS            (1e-3)        //!< Units of 1 mm/s.
+#define ANGA2RAD            (1e-5)        //!< Units of 0.01 mrad.
+#define GB2RPS              (5e-6)        //!< Units of 0.005 mrad/s.
+#define AB2MPS2             (1e-4)        //!< Units of 0.1 mm/s^2.
+#define GSFACTOR            (1e-6)        //!< Units of 1 ppm.
+#define ASFACTOR            (1e-6)        //!< Units of 1 ppm.
+#define GBA2RPS             (1e-6)        //!< Units of 0.001 mrad/s.
+#define ABA2MPS2            (1e-5)        //!< Units of 0.01 mm/s^2.
+#define GSAFACTOR           (1e-6)        //!< Units of 1 ppm.
+#define ASAFACTOR           (1e-6)        //!< Units of 1 ppm.
+#define GPSPOS2M            (1e-3)        //!< Units of 1 mm.
+#define GPSATT2RAD          (1e-4)        //!< Units of 0.1 mrad.
+#define GPSPOSA2M           (1e-4)        //!< Units of 0.1 mm.
+#define GPSATTA2RAD         (1e-5)        //!< Units of 0.01 mrad.
+#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
+#define DIFFAGE2SEC         (1e-2)        //!< Units of 0.01 s.
+#define REFPOS2M            (0.0012)      //!< Units of 1.2 mm.
+#define REFANG2RAD          (1e-4)        //!< Units of 0.1 mrad.
+#define OUTPOS2M            (1e-3)        //!< Units of 1 mm.
+#define ZVPOS2M             (1e-3)        //!< Units of 1 mm.
+#define ZVPOSA2M            (1e-4)        //!< Units of 0.1 mm.
+#define NSPOS2M             (1e-3)        //!< Units of 1 mm.
+#define NSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
+#define ALIGN2RAD           (1e-4)        //!< Units of 0.1 mrad.
+#define ALIGNA2RAD          (1e-5)        //!< Units of 0.01 mrad.
+#define SZVDELAY2S          (1.0)         //!< Units of 1.0 s.
+#define SZVPERIOD2S         (0.1)         //!< Units of 0.1 s.
+#define TOPSPEED2MPS        (0.5)         //!< Units of 0.5 m/s.
+#define NSDELAY2S           (0.1)         //!< Units of 0.1 s.
+#define NSPERIOD2S          (0.02)        //!< Units of 0.02 s.
+#define NSACCEL2MPS2        (0.04)        //!< Units of 0.04 m/s^2.
+#define NSSPEED2MPS         (0.1)         //!< Units of 0.1 m/s.
+#define NSRADIUS2M          (0.5)         //!< Units of 0.5 m.
+#define INITSPEED2MPS       (0.1)         //!< Units of 0.1 m/s.
+#define HLDELAY2S           (1.0)         //!< Units of 1.0 s.
+#define HLPERIOD2S          (0.1)         //!< Units of 0.1 s.
+#define STATDELAY2S         (1.0)         //!< Units of 1.0 s.
+#define STATSPEED2MPS       (0.01)        //!< Units of 1.0 cm/s.
+#define WSPOS2M             (1e-3)        //!< Units of 1 mm.
+#define WSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
+#define WSSF2PPM            (0.1)         //!< Units of 0.1 pulse per metre (ppm).
+#define WSSFA2PC            (0.002)       //!< Units of 0.002% of scale factor.
+#define WSDELAY2S           (0.1)         //!< Units of 0.1 s.
+#define WSNOISE2CNT         (0.1)         //!< Units of 0.1 count for wheel speed noise.
+#define UNDUL2M             (0.005)       //!< Units of 5 mm.
+#define DOPFACTOR           (0.1)         //!< Resolution of 0.1.
+#define OMNISTAR_MIN_FREQ   (1.52e9)      //!< (Hz) i.e. 1520.0 MHz.
+#define OMNIFREQ2HZ         (1000.0)      //!< Resolution of 1 kHz.
+#define SNR2DB              (0.2)         //!< Resolution of 0.2 dB.
+#define LTIME2SEC           (1.0)         //!< Resolution of 1.0 s.
+#define TEMPK_OFFSET        (203.15)      //!< Temperature offset in degrees K.
+#define ABSZERO_TEMPC       (-273.15)     //!< Absolute zero (i.e. 0 deg K) in deg C.
+
+// For more accurate and complete local coordinates
+#define FINEANG2RAD (1.74532925199433e-9) //!< Units of 0.1 udeg.
+#define ALT2M               (1e-3)        //!< Units of 1 mm.
+
+// For GPS supply voltage
+#define SUPPLYV2V           (0.1)         //!< Units of 0.1 V.
+
+// Mathematical constant definitions
+#ifndef M_PI
+#define M_PI (3.1415926535897932384626433832795)  //!< Pi.
+#endif
+#define DEG2RAD             (M_PI/180.0)  //!< Convert degrees to radians.
+#define RAD2DEG             (180.0/M_PI)  //!< Convert radians to degrees.
+#define POS_INT_24          (8388607)     //!< Maximum value of a two's complement 24 bit integer.
+#define NEG_INT_24          (-8388607)    //!< Minimum value of a two's complement 24 bit integer.
+#define INV_INT_24          (-8388608)    //!< Represents an invalid two's complement 24 bit integer.
+
+#define NCOM_COUNT_TOO_OLD  (150)         //!< Cycle counter for data too old.
+#define NCOM_STDCNT_MAX     (0xFF)        //!< Definition for the RTBNS accuracy counter.
+#define MIN_HORZ_SPEED      (0.07)        //!< 0.07 m/s hold distance.
+#define MIN_VERT_SPEED      (0.07)        //!< 0.07 m/s hold distance.
+#define SPEED_HOLD_FACTOR   (2.0)         //!< Hold distance when speed within 2 sigma of 0.
+#define MINUTES_IN_WEEK     (10080)       //!< Number of minutes in a week.
+
+// OmniStar status definitions
+#define NCOM_OMNI_STATUS_UNKNOWN      (0xFF)
+#define NCOM_OMNI_STATUS_VBSEXPIRED   (0x01)
+#define NCOM_OMNI_STATUS_VBSREGION    (0x02)
+#define NCOM_OMNI_STATUS_VBSNOBASE    (0x04)
+#define NCOM_OMNI_STATUS_HPEXPIRED    (0x08)
+#define NCOM_OMNI_STATUS_HPREGION     (0x10)
+#define NCOM_OMNI_STATUS_HPNOBASE     (0x20)
+#define NCOM_OMNI_STATUS_HPNOCONVERGE (0x40)
+#define NCOM_OMNI_STATUS_HPKEYINVALID (0x80)
+
+// GPS hardware status definitions
+#define NCOM_GPS_ANT_STATUS_BITMASK   (0x03)
+#define NCOM_GPS_ANT_STATUS_DONTKNOW  (0x03)
+#define NCOM_GPS_ANT_STATUS_BITSHIFT  (0)
+#define NCOM_GPS_ANT_POWER_BITMASK    (0x0C)
+#define NCOM_GPS_ANT_POWER_DONTKNOW   (0x0C)
+#define NCOM_GPS_ANT_POWER_BITSHIFT   (2)
+
+// GPS feature set 1 definitions
+#define NCOM_GPS_FEATURE_PSRDIFF      (0x01)
+#define NCOM_GPS_FEATURE_SBAS         (0x02)
+#define NCOM_GPS_FEATURE_OMNIVBS      (0x08)
+#define NCOM_GPS_FEATURE_OMNIHP       (0x10)
+#define NCOM_GPS_FEATURE_L1DIFF       (0x20)
+#define NCOM_GPS_FEATURE_L1L2DIFF     (0x40)
+
+// GPS feature set 2 definitions
+#define NCOM_GPS_FEATURE_GLONASS      (0x01)
+#define NCOM_GPS_FEATURE_GALILEO      (0x02)
+#define NCOM_GPS_FEATURE_RAWRNG       (0x04)
+#define NCOM_GPS_FEATURE_RAWDOP       (0x08)
+#define NCOM_GPS_FEATURE_RAWL1        (0x10)
+#define NCOM_GPS_FEATURE_RAWL2        (0x20)
+#define NCOM_GPS_FEATURE_RAWL5        (0x40)
+
+// GPS feature valid definition
+#define NCOM_GPS_FEATURE_VALID        (0x80)
+
+// The start of GPS time in a time_t style. In this version it is a constant, but this constant assumes that
+// the local machine uses 00:00:00 01/01/1970 as its Epoch time. If your machine is different then you need to
+// convert 00:00:00 06/01/1980 in to the local machine's time_t time.
+#define GPS_TIME_START_TIME_T         (315964800)
+
+// Second order filter class
+#define INPUT_JITTER_TOLERANCE     (0.01)  // i.e. 1%
+
+
+/*index*/
+#define		PI_SYNC             0
+#define		PI_TIME             1
+#define		PI_ACCEL_X          3
+#define		PI_ACCEL_Y          6
+#define		PI_ACCEL_Z          9
+#define		PI_ANG_RATE_X      12
+#define		PI_ANG_RATE_Y      15
+#define		PI_ANG_RATE_Z      18
+#define		PI_INS_NAV_MODE    21
+#define		PI_CHECKSUM_1      22
+#define		PI_POS_LAT         23
+#define		PI_POS_LON         31
+#define		PI_POS_ALT         39
+#define		PI_VEL_N           43
+#define		PI_VEL_E           46
+#define		PI_VEL_D           49
+#define		PI_ORIEN_H         52
+#define		PI_ORIEN_P         55
+#define		PI_ORIEN_R         58
+#define		PI_CHECKSUM_2      61
+#define		PI_CHANNEL_INDEX   62
+#define		PI_CHANNEL_STATUS  63
+#define		PI_SAT_NUM		   67
+#define		PI_RTK_STATUS	   68
+#define		PI_CHECKSUM_3      71
+
+/*RTK IMU status check*/
+#define		RTK_IMU_OK		0
+#define		IMU_STATUS_ERR	1
+#define		RTK_STATUS_ERR	2
+#define		UNKNOWN			0xFF
+
+namespace iv {
+	namespace perception {
+		class GPSSensor : public iv::perception::Sensor
+		{
+		public:
+			GPSSensor();
+			~GPSSensor();
+
+			// 通过 Sensor 继承
+			virtual void start() override;
+
+			virtual void stop() override;
+
+			virtual bool isRunning() const override;
+
+			virtual void processSensor() override;
+            void convertStrToUnChar(char* str, unsigned char* UnChar);
+
+			/* gps 惯导 数据*/
+			typedef void
+			(sig_cb_gps_sensor_data)(iv::GPSData);
+
+		private:
+			boost::signals2::signal<sig_cb_gps_sensor_data>* signal_gps_data;	//GPS 惯导数据信号
+
+        private:
+            void * mpagpsimu;
+
+        public:
+            void UpdateGPSIMU(iv::gps::gpsimu xgpsimu);
+		};
+
+
+	}
+}
+
+#endif // !_IV_PERCEPTION_SENSOR_GPS_

src/decition/common/perception_sf/sensor_lidar.cpp

@@ -1,143 +1,143 @@
-#include <perception_sf/sensor_lidar.h>
-
-namespace iv {
-namespace perception {
-    LiDARSensor * gipl;
-
-}
-}
-
-iv::perception::LiDARSensor::LiDARSensor() {
-    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
-    mobs = lidar_obs;
-    mbRun = false;
-    gipl = this;
-	signal_lidar_obstacle_grid = createSignal<sig_cb_lidar_sensor_obstacle_grid>();
-//    signal_fusion_obstacle_grid = createSignal<sig_cb_fusion_sensor_obstacle_grid>();
-    JiGuangLeiDa = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
-}
-iv::perception::LiDARSensor::~LiDARSensor() {
-
-}
-
-void ListenPer(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-//    qDebug("size is %d",nSize);
-
-    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
-
-
-    iv::Perception::PerceptionOutput * pdata = (iv::Perception::PerceptionOutput *)strdata;
-    int nCount = nSize/sizeof(iv::Perception::PerceptionOutput);
-    int i;
-    for(i=0;i<nCount;i++)
-    {
-        iv::Perception::PerceptionOutput temp;
-        memcpy(&temp,pdata,sizeof(iv::Perception::PerceptionOutput));
-        lidar_per->push_back(temp);
-        pdata++;
-    }
-    iv::perception::gipl->UpdatePer(lidar_per);
-
-  //  gw->UpdateOBS(lidar_obs);
-}
-
-
-
-void ListenFusion(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
-{
-    std::shared_ptr<iv::fusion::fusionobjectarray> xfusion (new iv::fusion::fusionobjectarray);
-
-    if(nSize<1)return;
-    if(false == xfusion->ParseFromArray(strdata,nSize))
-    {
-        std::cout<<" Listenesrfront fail."<<std::endl;
-        return;
-    }
-    else{
-        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
-    }
-
-//    for(int i=0;i<xfusion->obj_size();i++)
-//    {
-//        std::cout<<"obj     id:   "<<xfusion->obj(i).id()<<std::endl;
-//    }
-
-
-
-    iv::perception::gipl->updataFusion(xfusion);
-
-
-}
-
-
-
-void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-//    qDebug("size is %d",nSize);
-
-    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
-
-
-    iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
-    int nCount = nSize/sizeof(iv::ObstacleBasic);
-    int i;
-    for(i=0;i<nCount;i++)
-    {
-        iv::ObstacleBasic temp;
-        memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
-        lidar_obs->push_back(temp);
-        pdata++;
-    }
-    iv::perception::gipl->UpdateOBS(lidar_obs);
-
-  //  gw->UpdateOBS(lidar_obs);
-}
-
-
-void iv::perception::LiDARSensor::start() {
-
-
-    iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
-//    mpa = new adcmemshare("lidar_obs",20000000,3);
-//    mpa->listenmsg(ListenOBS);
-
-    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
-//    mpb = new adcmemshare("lidar_per",20000000,3);
-//    mpb->listenmsg(ListenPer);
-
-
-    // txs
-
-    iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
-
-    //txs
-
-    mbRun = true;
-    return;
-
-//	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::LiDARSensor::processSensor, this));
-//	thread_sensor_run_collect = new boost::thread(boost::bind(&iv::perception::LiDARSensor::lidarcollect, this));
-//	std::cout << "LiDAR thread Run" << std::endl;
-
-}
-
-void iv::perception::LiDARSensor::stop() {
-    delete mpa;
-    mbRun = false;
-    return;
-	thread_sensor_run_->interrupt();
-	thread_sensor_run_->join();
-	thread_sensor_run_collect->interrupt();
-	thread_sensor_run_collect->join();
-	std::cout << "LiDAR thread Stop" << std::endl;
-
-
-}
-
-bool iv::perception::LiDARSensor::isRunning() const {
-    return mbRun;
-	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)) && thread_sensor_run_collect != NULL && !thread_sensor_run_collect->timed_join(boost::posix_time::milliseconds(10)));
-}
+#include <perception_sf/sensor_lidar.h>
+
+namespace iv {
+namespace perception {
+    LiDARSensor * gipl;
+
+}
+}
+
+iv::perception::LiDARSensor::LiDARSensor() {
+    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
+    mobs = lidar_obs;
+    mbRun = false;
+    gipl = this;
+	signal_lidar_obstacle_grid = createSignal<sig_cb_lidar_sensor_obstacle_grid>();
+//    signal_fusion_obstacle_grid = createSignal<sig_cb_fusion_sensor_obstacle_grid>();
+    JiGuangLeiDa = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
+}
+iv::perception::LiDARSensor::~LiDARSensor() {
+
+}
+
+void ListenPer(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+//    qDebug("size is %d",nSize);
+
+    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
+
+
+    iv::Perception::PerceptionOutput * pdata = (iv::Perception::PerceptionOutput *)strdata;
+    int nCount = nSize/sizeof(iv::Perception::PerceptionOutput);
+    int i;
+    for(i=0;i<nCount;i++)
+    {
+        iv::Perception::PerceptionOutput temp;
+        memcpy(&temp,pdata,sizeof(iv::Perception::PerceptionOutput));
+        lidar_per->push_back(temp);
+        pdata++;
+    }
+    iv::perception::gipl->UpdatePer(lidar_per);
+
+  //  gw->UpdateOBS(lidar_obs);
+}
+
+
+
+void ListenFusion(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    std::shared_ptr<iv::fusion::fusionobjectarray> xfusion (new iv::fusion::fusionobjectarray);
+
+    if(nSize<1)return;
+    if(false == xfusion->ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" Listenesrfront fail."<<std::endl;
+        return;
+    }
+    else{
+        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
+    }
+
+//    for(int i=0;i<xfusion->obj_size();i++)
+//    {
+//        std::cout<<"obj     id:   "<<xfusion->obj(i).id()<<std::endl;
+//    }
+
+
+
+    iv::perception::gipl->updataFusion(xfusion);
+
+
+}
+
+
+
+void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+//    qDebug("size is %d",nSize);
+
+    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
+
+
+    iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
+    int nCount = nSize/sizeof(iv::ObstacleBasic);
+    int i;
+    for(i=0;i<nCount;i++)
+    {
+        iv::ObstacleBasic temp;
+        memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
+        lidar_obs->push_back(temp);
+        pdata++;
+    }
+    iv::perception::gipl->UpdateOBS(lidar_obs);
+
+  //  gw->UpdateOBS(lidar_obs);
+}
+
+
+void iv::perception::LiDARSensor::start() {
+
+
+ //   iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
+//    mpa = new adcmemshare("lidar_obs",20000000,3);
+//    mpa->listenmsg(ListenOBS);
+
+//    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
+//    mpb = new adcmemshare("lidar_per",20000000,3);
+//    mpb->listenmsg(ListenPer);
+
+
+    // txs
+
+    iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
+
+    //txs
+
+    mbRun = true;
+    return;
+
+//	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::LiDARSensor::processSensor, this));
+//	thread_sensor_run_collect = new boost::thread(boost::bind(&iv::perception::LiDARSensor::lidarcollect, this));
+//	std::cout << "LiDAR thread Run" << std::endl;
+
+}
+
+void iv::perception::LiDARSensor::stop() {
+    delete mpa;
+    mbRun = false;
+    return;
+	thread_sensor_run_->interrupt();
+	thread_sensor_run_->join();
+	thread_sensor_run_collect->interrupt();
+	thread_sensor_run_collect->join();
+	std::cout << "LiDAR thread Stop" << std::endl;
+
+
+}
+
+bool iv::perception::LiDARSensor::isRunning() const {
+    return mbRun;
+	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)) && thread_sensor_run_collect != NULL && !thread_sensor_run_collect->timed_join(boost::posix_time::milliseconds(10)));
+}

src/decition/common/perception_sf/sensor_lidar.h

@@ -1,102 +1,102 @@
-#pragma once
-/**
-* 激光雷达
-*/
-#ifndef _IV_PERCEPTION_LIDARSENSOR_
-#define	_IV_PERCEPTION_LIDARSENSOR_
-
-#define Lidar_Pi 3.1415926535897932384626433832795
-#define Lidar32 (unsigned long)3405883584//192.168.1.203
-#define Lidar_roll_ang -3.15*Lidar_Pi/180.0
-
-#include <common_sf/boost.h>
-#include <common_sf/lidar.h>
-#include <perception_sf/sensor.h>
-#include "fusionobjectarray.pb.h"
-#include <mutex>
-#include <stdlib.h>
-#include <stdio.h>
-#include <cmath>
-#include <QMutex>
-
-#include "modulecomm.h"
-
-namespace iv {
-	//CRITICAL_SECTION cs;
-	namespace perception {
-		class LiDARSensor : public iv::perception::Sensor
-		{
-		public:
-			LiDARSensor() ;
-			~LiDARSensor();
-
-			virtual void start() override;
-
-			virtual void stop() override;
-
-			virtual bool isRunning() const override;
-
-			virtual void processSensor() override;	
-
-            void UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs);
-
-            void UpdatePer(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per);
-
-            // txs   12.16
-            void updataFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
-            // txs  12.16
-
-            QMutex mMutex;
-
-            std::shared_ptr<std::vector<iv::ObstacleBasic>> mobs;
-
-            // txs   12.16
-            std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;
-            // txs   12.16
-
-
-            void lidarcollect();
-             void convertStrToUnChar(char* str, unsigned char* UnChar);
-
-			boost::thread* thread_sensor_run_collect;			//传感器运行的线程2
-
-			/* 激光雷达障碍物*/
-			typedef void
-			(sig_cb_lidar_sensor_obstacle_grid)(iv::LidarGridPtr);
-
-//            typedef void
-//            (sig_cb_fusion_sensor_obstacle_grid)(iv::Obs_grid*);
-
-		
-		private:
-			boost::signals2::signal<sig_cb_lidar_sensor_obstacle_grid>* signal_lidar_obstacle_grid;	//激光雷达栅格
-
-  //          boost::signals2::signal<sig_cb_lidar_sensor_obstacle_grid>* signal_fusion_obstacle_grid;
-
-            unsigned char recvBuf1[320][1207];
-            unsigned char recvBuf2[320][1207];
-            int buf1len;
-            int buf2len;
-            int dx;
-            int dy;
-            bool bufuse_1 = false;//true:数据处理中，不能写入  false：完成处理，可以写入
-            bool bufuse_2 = false;
-            iv::ObsLiDAR JiGuangLeiDa;
-            iv::Obs_grid obs_grid[iv::grx + 1][iv::gry + 1];
-            iv::LidarGridPtr ptr;
-
-            // txs 12.16
-   //        iv::Obs_grid* fusion_ptr_;
-            iv::LidarGridPtr fusion_ptr_;
-            // txs 12.16
-
-            void * mpa;
-            void * mpb;
-            bool mbRun;
-
-		};
-
-	}
-}
-
-#endif // !_IV_PERCEPTION_LIDARSENSOR_
+#pragma once
+/**
+* 激光雷达
+*/
+#ifndef _IV_PERCEPTION_LIDARSENSOR_
+#define	_IV_PERCEPTION_LIDARSENSOR_
+
+#define Lidar_Pi 3.1415926535897932384626433832795
+#define Lidar32 (unsigned long)3405883584//192.168.1.203
+#define Lidar_roll_ang -3.15*Lidar_Pi/180.0
+
+#include <common_sf/boost.h>
+#include <common_sf/lidar.h>
+#include <perception_sf/sensor.h>
+#include "fusionobjectarray.pb.h"
+#include <mutex>
+#include <stdlib.h>
+#include <stdio.h>
+#include <cmath>
+#include <QMutex>
+
+#include "modulecomm.h"
+
+namespace iv {
+	//CRITICAL_SECTION cs;
+	namespace perception {
+		class LiDARSensor : public iv::perception::Sensor
+		{
+		public:
+			LiDARSensor() ;
+			~LiDARSensor();
+
+			virtual void start() override;
+
+			virtual void stop() override;
+
+			virtual bool isRunning() const override;
+
+			virtual void processSensor() override;	
+
+            void UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs);
+
+            void UpdatePer(std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per);
+
+            // txs   12.16
+            void updataFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
+            // txs  12.16
+
+            QMutex mMutex;
+
+            std::shared_ptr<std::vector<iv::ObstacleBasic>> mobs;
+
+            // txs   12.16
+            std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;
+            // txs   12.16
+
+
+            void lidarcollect();
+             void convertStrToUnChar(char* str, unsigned char* UnChar);
+
+			boost::thread* thread_sensor_run_collect;			//传感器运行的线程2
+
+			/* 激光雷达障碍物*/
+			typedef void
+			(sig_cb_lidar_sensor_obstacle_grid)(iv::LidarGridPtr);
+
+//            typedef void
+//            (sig_cb_fusion_sensor_obstacle_grid)(iv::Obs_grid*);
+
+		
+		private:
+			boost::signals2::signal<sig_cb_lidar_sensor_obstacle_grid>* signal_lidar_obstacle_grid;	//激光雷达栅格
+
+  //          boost::signals2::signal<sig_cb_lidar_sensor_obstacle_grid>* signal_fusion_obstacle_grid;
+
+            unsigned char recvBuf1[320][1207];
+            unsigned char recvBuf2[320][1207];
+            int buf1len;
+            int buf2len;
+            int dx;
+            int dy;
+            bool bufuse_1 = false;//true:数据处理中，不能写入  false：完成处理，可以写入
+            bool bufuse_2 = false;
+            iv::ObsLiDAR JiGuangLeiDa;
+            iv::Obs_grid obs_grid[iv::grx + 1][iv::gry + 1];
+            iv::LidarGridPtr ptr;
+
+            // txs 12.16
+   //        iv::Obs_grid* fusion_ptr_;
+            iv::LidarGridPtr fusion_ptr_;
+            // txs 12.16
+
+            void * mpa;
+            void * mpb;
+            bool mbRun;
+
+		};
+
+	}
+}
+
+#endif // !_IV_PERCEPTION_LIDARSENSOR_

src/decition/common/perception_sf/sensor_radar.cpp

@@ -1,55 +1,55 @@
-#include <perception_sf/sensor_radar.h>
-
-
-
-extern std::string gstrmemradar;
-
-
-namespace iv {
-namespace perception {
-    RadarSensor * gRadar;
-
-    void ListenRadar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-    {
-        if(nSize<1)return;
-        iv::radar::radarobjectarray xobj;
-
-        if(false == xobj.ParseFromArray(strdata,nSize))
-        {
-            std::cout<<"ListenenRadar fail."<<std::endl;
-            return;
-        }
-        gRadar->UpdateRADAR(xobj);
-    }
-}
-}
-
-iv::perception::RadarSensor::RadarSensor() {
-	signal_radar_obstacle = createSignal<sig_cb_radar_sensor_obstacle>();
-
-    iv::perception::gRadar = this;
-
-    mparadar = iv::modulecomm::RegisterRecv(gstrmemradar.data(),iv::perception::ListenRadar);
-//    mparadar = new adcmemshare("radar",10*sizeof(iv::ObstacleBasic)*64,10);
-//    mparadar->listenmsg(iv::perception::ListenRadar);
-}
-
-
-void iv::perception::RadarSensor::start()
-{
-	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::RadarSensor::processSensor, this));
-	std::cout << "Radar Thread Run" << std::endl;
-}
-
-void iv::perception::RadarSensor::stop()
-{
-	thread_sensor_run_->interrupt();
-	thread_sensor_run_->join();
-	std::cout << "Radar Thread Stop" << std::endl;
-}
-
-bool iv::perception::RadarSensor::isRunning() const
-{
-	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
-}
-
+#include <perception_sf/sensor_radar.h>
+
+
+
+extern std::string gstrmemradar;
+
+
+namespace iv {
+namespace perception {
+    RadarSensor * gRadar;
+
+    void ListenRadar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+    {
+        if(nSize<1)return;
+        iv::radar::radarobjectarray xobj;
+
+        if(false == xobj.ParseFromArray(strdata,nSize))
+        {
+            std::cout<<"ListenenRadar fail."<<std::endl;
+            return;
+        }
+        gRadar->UpdateRADAR(xobj);
+    }
+}
+}
+
+iv::perception::RadarSensor::RadarSensor() {
+	signal_radar_obstacle = createSignal<sig_cb_radar_sensor_obstacle>();
+
+    iv::perception::gRadar = this;
+
+    mparadar = iv::modulecomm::RegisterRecv(gstrmemradar.data(),iv::perception::ListenRadar);
+//    mparadar = new adcmemshare("radar",10*sizeof(iv::ObstacleBasic)*64,10);
+//    mparadar->listenmsg(iv::perception::ListenRadar);
+}
+
+
+void iv::perception::RadarSensor::start()
+{
+	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::RadarSensor::processSensor, this));
+	std::cout << "Radar Thread Run" << std::endl;
+}
+
+void iv::perception::RadarSensor::stop()
+{
+	thread_sensor_run_->interrupt();
+	thread_sensor_run_->join();
+	std::cout << "Radar Thread Stop" << std::endl;
+}
+
+bool iv::perception::RadarSensor::isRunning() const
+{
+	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
+}
+

src/decition/common/perception_sf/sensor_radar.h

@@ -1,37 +1,37 @@
-#pragma once
-#ifndef _IV_PERCEPTION_SENSOR_RADAR_
-#define _IV_PERCEPTION_SENSOR_RADAR_
-
-#include <common_sf/boost.h>
-#include <perception_sf/sensor.h>
-
-#include "radarobjectarray.pb.h"
-
-#include  "modulecomm.h"
-namespace iv {
-	namespace perception {
-		class RadarSensor : public iv::perception::Sensor {
-		public:
-			RadarSensor();
-			~RadarSensor();
-			// 通过 Sensor 继承
-			virtual void start() override;
-			virtual void stop() override;
-			virtual bool isRunning() const override;
-			virtual void processSensor() override;
-
-			/* 毫米波雷达障碍物*/
-			typedef void
-			(sig_cb_radar_sensor_obstacle)(iv::ObsRadar);
-
-		private:
-			boost::signals2::signal<sig_cb_radar_sensor_obstacle>* signal_radar_obstacle;//毫米波雷达障碍物信号
-
-        private:
-            void * mparadar;
-        public:
-            void UpdateRADAR(iv::radar::radarobjectarray xarray);
-		};
-	}
-}
-#endif // !_IV_PERCEPTION_SENSOR_RADAR_
+#pragma once
+#ifndef _IV_PERCEPTION_SENSOR_RADAR_
+#define _IV_PERCEPTION_SENSOR_RADAR_
+
+#include <common_sf/boost.h>
+#include <perception_sf/sensor.h>
+
+#include "radarobjectarray.pb.h"
+
+#include  "modulecomm.h"
+namespace iv {
+	namespace perception {
+		class RadarSensor : public iv::perception::Sensor {
+		public:
+			RadarSensor();
+			~RadarSensor();
+			// 通过 Sensor 继承
+			virtual void start() override;
+			virtual void stop() override;
+			virtual bool isRunning() const override;
+			virtual void processSensor() override;
+
+			/* 毫米波雷达障碍物*/
+			typedef void
+			(sig_cb_radar_sensor_obstacle)(iv::ObsRadar);
+
+		private:
+			boost::signals2::signal<sig_cb_radar_sensor_obstacle>* signal_radar_obstacle;//毫米波雷达障碍物信号
+
+        private:
+            void * mparadar;
+        public:
+            void UpdateRADAR(iv::radar::radarobjectarray xarray);
+		};
+	}
+}
+#endif // !_IV_PERCEPTION_SENSOR_RADAR_

src/decition/common/platform/dataformat.h

@@ -40,7 +40,7 @@
 #define reerror (0x02)//错误
 #define VINrepetition (0x03)//VIN重复
 #define reserves (0x40)//保留
-//#define comment (0xFE)//命令
+#define comment (0xFE)//命令
 
 #include <string>
 

src/decition/common/platform/platform.cpp

@@ -1,2069 +1,2069 @@
-#include <decition/adc_tools/compute_00.h>
-#include <decition/decide_gps_00.h>
-#include <decition/adc_tools/gps_distance.h>
-#include <decition/decition_type.h>
-#include <decition/adc_tools/transfer.h>
-#include <common/constants.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-#include <control/can.h>
-using namespace std;
-
-#define PI (3.1415926535897932384626433832795)
-
-iv::decition::Compute00::Compute00() {
-
-}
-iv::decition::Compute00::~Compute00() {
-
-}
-
-
-double iv::decition::Compute00::angleLimit = 700;
-double iv::decition::Compute00::lastEA = 0;
-double iv::decition::Compute00::lastEP = 0;
-double iv::decition::Compute00::decision_Angle = 0;
-double iv::decition::Compute00::lastAng = 0;
-int iv::decition::Compute00::lastEsrID = -1;
-int  iv::decition::Compute00::lastEsrCount = 0;
-int iv::decition::Compute00::lastEsrIDAvoid = -1;
-int  iv::decition::Compute00::lastEsrCountAvoid = 0;
-
-iv::GPS_INS  iv::decition::Compute00::nearTpoint;
-iv::GPS_INS  iv::decition::Compute00::farTpoint;
-double  iv::decition::Compute00::bocheAngle;
-
-
-
-iv::GPS_INS  iv::decition::Compute00::dTpoint0;
-iv::GPS_INS  iv::decition::Compute00::dTpoint1;
-iv::GPS_INS  iv::decition::Compute00::dTpoint2;
-iv::GPS_INS  iv::decition::Compute00::dTpoint3;
-double  iv::decition::Compute00::dBocheAngle;
-
-
-std::vector<int> lastEsrIdVector;
-std::vector<int> lastEsrCountVector;
-
-
-
-int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
-{
-    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = gpsMap.size() - 1;
-    static double FrontTotalFive=0,FrontAveFive=0,BackAveFive=0,BackTotalFive=0;
-    static int FrontCount=0,BackCount=0;
-    static int CurrentIndex=0,MarkingIndex=0,MarkingCount=0;
-    int MarkedIndex=0,CurveContinue=0;
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        (*gpsMap[i]).roadMode=5;
-    }
-    for (int j = startIndex; j < (endIndex-10); j+=10)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        for(int front_k=i;front_k<i+5;front_k++)
-        {
-            if(fabs(((*gpsMap[front_k]).ins_heading_angle)-((*gpsMap[i]).ins_heading_angle))<10)
-            {
-                   FrontTotalFive+=(*gpsMap[front_k]).ins_heading_angle;
-                   FrontCount++;
-            }
-
-        }
-        i+=5;
-        FrontAveFive=FrontTotalFive/FrontCount;
-        FrontTotalFive=0;
-        FrontCount=0;
-        for(int back_k=i;back_k<i+5;back_k++)
-        {
-            if(fabs((*gpsMap[back_k]).ins_heading_angle-(*gpsMap[i]).ins_heading_angle)<10)
-            {
-                   BackTotalFive+=(*gpsMap[back_k]).ins_heading_angle;
-                   BackCount++;
-            }
-
-        }
-        i+=5;
-        CurrentIndex=i-1;
-        BackAveFive=BackTotalFive/BackCount;
-        BackTotalFive=0;
-        BackCount=0;
-        if(fabs(FrontAveFive-BackAveFive)<50)
-        {
-                   if(fabs(FrontAveFive-BackAveFive)>3)
-                   {
-                        CurveContinue+=1;
-                        if(CurveContinue>=2)
-                        {
-                            MarkingCount=0;
-                            for(MarkingIndex=CurrentIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
-                            {
-
-                                if(MarkingCount<100)
-                                {
-                                     if((BackAveFive-FrontAveFive)<3)
-                                     {
-                                           (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
-                                     }
-                                     else if((BackAveFive-FrontAveFive)>3)
-                                     {
-                                           (*gpsMap[MarkingIndex]).roadMode=15;
-                                     }
-
-                                }
-                                else if((MarkingCount>=100)&&(MarkingCount<150))
-                                {
-                                     (*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米,1
-                                }
-                                else if((MarkingCount>=150)&&(MarkingCount<320))
-                                {
-                                     (*gpsMap[MarkingIndex]).roadMode=5;   //低速，20米,5
-                                }
-                                else if((MarkingCount>=320)&&(MarkingCount<620))
-                                {
-                                     (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
-                                }
-                                else if(MarkingCount>=620)
-                                {
-                                     (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
-                                }
-
-                                MarkingCount++;
-                            }
-                            MarkedIndex=CurrentIndex;
-                        }
-                   }
-                   else if(fabs(FrontAveFive-BackAveFive)<=3)
-                   {
-                        CurveContinue=0;
-                   }
-        }
-        FrontAveFive=0;
-        BackAveFive=0;
-    }
-
-    if(MarkedIndex<endIndex)
-    {
-        MarkingCount=0;
-        for(MarkingIndex=endIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
-        {
-
-            if(MarkingCount<100)
-            {
-                if((BackAveFive-FrontAveFive)<3)
-                {
-                      (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
-                }
-                else if((BackAveFive-FrontAveFive)>3)
-                {
-                      (*gpsMap[MarkingIndex]).roadMode=15;
-                }
-            }
-            else if((MarkingCount>=100)&&(MarkingCount<150))
-            {
-                 (*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米
-            }
-            else if((MarkingCount>=150)&&(MarkingCount<320))
-            {
-                 (*gpsMap[MarkingIndex]).roadMode=5;   //低速，30米
-            }
-            else if((MarkingCount>=320)&&(MarkingCount<620))
-            {
-                 (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
-            }
-            else if(MarkingCount>=620)
-            {
-                 (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
-            }
-
-            MarkingCount++;
-        }
-    }
-
-
-    return 1;
-}
-
-
-
-//首次找点
-
-int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
-{
-    int index = -1;
-    //	DecideGps00().minDis = iv::MaxValue;
-    float minDis = 10;
-    double maxAng = iv::MaxValue;
-
-    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = gpsMap.size() - 1;
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
-                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
-                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
-                )
-        {
-            index = i;
-            minDis = tmpdis;
-            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
-            maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
-        }
-    }
-
-    DecideGps00().maxAngle=maxAng;
-    DecideGps00().minDis=minDis;
-    return index;
-}
-
-//search pathpoint
-int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
-{
-    int index = -1;
-    float minDis = 10;
-    double maxAng = iv::MaxValue;
-    int map_size=gpsMap.size();
-    int preDistance=max(100,(int)(rp.speed*10));
-        preDistance=min(500,preDistance);
-
-    int startIndex = max((int)(lastIndex - 100),(int)(lastIndex-map_size));     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = min((int)(lastIndex + preDistance ),(int)(lastIndex+map_size));
-
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + map_size) % map_size;
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
-            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
-            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
-            )
-        {
-            index = i;
-            minDis = tmpdis;
-            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
-            maxAng = min(maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
-        }
-    }
-
-
-    DecideGps00().maxAngle=maxAng;
-    DecideGps00().minDis=minDis;
-    return index;
-}
-
-
-double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
-{
-    double sum_x = 0;
-    double sum_y = 0;
-
-    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
-    {
-        sum_x += farTrace[i].x;
-        sum_y += abs(farTrace[i].y);
-    }
-    double average_y = sum_y / min(5, (int)farTrace.size());
-    double average_x = sum_x / min(5, (int)farTrace.size());
-
-
-    return atan(average_x / average_y) / PI * 180;
-}
-
-
-
-double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
-{
-    double sum_x = 0;
-    double sum_y = 0;
-
-    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
-    {
-        sum_x += farTrace[i].x;
-        sum_y += abs(farTrace[i].y);
-    }
-    double average_y = sum_y / min(5, (int)farTrace.size());
-    double average_x = sum_x / min(5, (int)farTrace.size());
-
-
-    return atan(average_x + avoidX / average_y) / PI * 180;
-}
-
-
-
-
-
-
-double iv::decition::Compute00::getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-
-    //   double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1;  // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-
-    if(transferPieriod&& !transferPieriod2){
-        DEang = 200;
-        DEPos = 150;
-    }
-
-    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-    double PreviewDistance;//预瞄距离
-    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-    if(changeRoad ||transferPieriod){
-        PreviewDistance=PreviewDistance+avoidX;
-    }
-    if(realSpeed <15){
-        PreviewDistance = max(4.0, realSpeed *0.4) ;
-    }
-
-    if (gpsTrace[0].v1 == 1)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
-    {
-        KEang = 18; KEPos = 50; PreviewDistance = 3;
-    }
-    else if (gpsTrace[0].v1 == 7)
-    {
-        KEang = 20; KEPos = 50; PreviewDistance = 4;
-    }
-
-
-    if (realSpeed > 40)	KEang = 10; KEPos = 8;
-    if (realSpeed > 50) KEang = 5;
-
-    double sumdis = 0;
-    int gpsIndex = 0;
-    std::vector<Point2D> farTrace;
-
-
-
-
-
-
-    for (int i = 1; i < gpsTrace.size() - 1; i++)
-    {
-        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
-        if (sumdis > PreviewDistance)
-        {
-            gpsIndex = i;
-            break;
-        }
-    }
-
-
-
-
-    EPos = gpsTrace[gpsIndex].x;
-
-    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
-        farTrace.push_back(gpsTrace[gpsIndex]);
-    }
-    if (farTrace.size() == 0) {
-        EAng = 0;
-    }
-    else {
-        EAng = getAveDef(farTrace);
-    }
-
-    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-    return ang;
-}
-
-
-
-
-int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
-{
-    int index = 1;
-    double sumdis = 0;
-    while (index < gpsTrace.size() && sumdis < realSpeed)
-        sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);
-
-    if (index == gpsTrace.size())
-        return index - 1;
-
-    if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
-        index--;
-    return index;
-}
-
-iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    float xiuzheng=0;
-    if(!ServiceCarStatus.useMobileEye){
-        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
-    }
-
-
-    for (int j = 0; j < gpsTrace.size(); j++)
-    {
-        double sumx1 = 0, sumy1 = 0, count1 = 0;
-        double sumx2 = 0, sumy2 = 0, count2 = 0;
-        for (int k = max(0, j - 4); k <= j; k++)
-        {
-            count1 = count1 + 1;
-            sumx1 += gpsTrace[k].x;
-            sumy1 += gpsTrace[k].y;
-        }
-        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
-        {
-            count2 = count2 + 1;
-            sumx2 += gpsTrace[k].x;
-            sumy2 += gpsTrace[k].y;
-        }
-        sumx1 /= count1; sumy1 /= count1;
-        sumx2 /= count2; sumy2 /= count2;
-
-        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
-
-        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
-        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
-
-        Point2D ptLeft(carFrontx + Veh_Width / 2 * cos(anglevalue + PI / 2),
-                       carFronty + Veh_Width / 2 * sin(anglevalue + PI / 2));
-
-        Point2D ptRight(carFrontx + Veh_Width / 2 * cos(anglevalue - PI / 2),
-                        carFronty + Veh_Width / 2 * sin(anglevalue - PI / 2));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
-        {
-            int count = 0;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
-                int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
-                obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
-                obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
-
-                isRemove = true;
-                //		DecideGps00().lidarDistance = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistance = obsPoint.y;
-    return obsPoint;
-}
-
-//1220
-iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    float xiuzheng=0;
-    if(!ServiceCarStatus.useMobileEye){
-        xiuzheng=0-ServiceCarStatus.msysparam.rearLidarGpsXiuzheng;
-    }
-    for (int j = 0; j < gpsTrace.size(); j++)
-    {
-        double sumx1 = 0, sumy1 = 0, count1 = 0;
-        double sumx2 = 0, sumy2 = 0, count2 = 0;
-        for (int k = max(0, j - 4); k <= j; k++)
-        {
-            count1 = count1 + 1;
-            sumx1 += gpsTrace[k].x;
-            sumy1 += gpsTrace[k].y;
-        }
-        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
-        {
-            count2 = count2 + 1;
-            sumx2 += gpsTrace[k].x;
-            sumy2 += gpsTrace[k].y;
-        }
-        sumx1 /= count1; sumy1 /= count1;
-        sumx2 /= count2; sumy2 /= count2;
-
-        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
-
-        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
-        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
-
-        Point2D ptLeft(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue + PI / 2),
-                       carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue + PI / 2));
-
-        Point2D ptRight(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue - PI / 2),
-                        carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue - PI / 2));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y<(0-ServiceCarStatus.msysparam.rearGpsXiuzheng) )
-        {
-            int count = 0;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-
-                dx=grx-dx;//1227
-
-                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-
-                dx=grx-dx;//1227
-                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                isRemove = true;
-                //		DecideGps00().lidarDistance = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistance = obsPoint.y;
-    return obsPoint;
-}
-
-
-iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    for (int j = 0; j < gpsTrace.size(); j++)
-    {
-        double sumx1 = 0, sumy1 = 0, count1 = 0;
-        double sumx2 = 0, sumy2 = 0, count2 = 0;
-        for (int k = max(0, j - 4); k <= j; k++)
-        {
-            count1 = count1 + 1;
-            sumx1 += gpsTrace[k].x;
-            sumy1 += gpsTrace[k].y;
-        }
-        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
-        {
-            count2 = count2 + 1;
-            sumx2 += gpsTrace[k].x;
-            sumy2 += gpsTrace[k].y;
-        }
-        sumx1 /= count1; sumy1 /= count1;
-        sumx2 /= count2; sumy2 /= count2;
-
-        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
-
-        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
-        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
-
-        //1127 fanwei xiuzheng
-        float buchang=0;
-        Point2D ptLeft(carFrontx + (Veh_Width+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
-                       carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
-
-        Point2D ptRight(carFrontx +  (Veh_Width+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
-                        carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
-        {
-            int count = 0;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
-                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                isRemove = true;
-                DecideGps00().lidarDistanceAvoid = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistanceAvoid = obsPoint.y;
-
-    return obsPoint;
-}
-
-
-
-
-//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
-//	bool isRemove = false;
-//
-//	for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-//	{
-//
-//		for (int i = 0; i < esrRadars.size(); i++)
-//			if ((esrRadars[i].nomal_y) != 0)
-//			{
-//				double xxx = esrRadars[i].nomal_x + Esr_Offset;
-//				double yyy = esrRadars[i].nomal_y;
-//
-//				if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-//				{
-//
-//					if (lastEsrID == (esrRadars[i]).esr_ID)
-//					{
-//						lastEsrCount++;
-//					}
-//					else
-//					{
-//						lastEsrCount = 0;
-//					}
-//
-//					if (lastEsrCount >= 3)
-//					{
-//						return i;
-//					}
-//
-//					lastEsrID = (esrRadars[i]).esr_ID;
-//				}
-//			}
-//	}
-//	return -1;
-//}
-
-
-
-
-int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint) {
-    bool isRemove = false;
-
-    float xiuzheng=0;
-    if(!ServiceCarStatus.useMobileEye){
-        xiuzheng=ServiceCarStatus.msysparam.radarGpsXiuzheng;
-    }
-
-    float fxiuzhengCs = DecideGps00().xiuzhengCs;
-    int nsize = gpsTrace.size();
-    for (int j = 1; j < nsize - 1 && !isRemove; j++)
-    {
-
-        for (int i = 0; i < 64; i++)
-            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
-            {
-                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-                double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ xiuzheng;
-
-                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
-                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
-
-//优化
-//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*Veh_Width / 2.0+DecideGps00().xiuzhengCs)){
-//                    *esrPathpoint = j;
-//                    return i;
-//                }
-
-
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
-                {
-
-                    if (lastEsrID == i)
-                    {
-                        lastEsrCount++;
-                    }
-                    else
-                    {
-                        lastEsrCount = 0;
-                    }
-
-                    if(yyy>50 ){
-                        if (lastEsrCount >=200)
-                        {
-                            return i;
-                        }
-                    }
-                    else if (lastEsrCount >= 1)
-                    {
-                        return i;
-                    }
-
-                    lastEsrID = i;
-                }
-            }
-    }
-    return -1;
-}
-
-
-
-
-int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
-    bool isRemove = false;
-
-    float xiuzheng = ServiceCarStatus.msysparam.rearRadarGpsXiuzheng;
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        for (int i = 0; i < 64; i++)
-            if ((ServiceCarStatus.obs_rear_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_rear_radar[i].valid))
-            {
-                double xxx = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_x + Esr_Offset);
-                double yyy = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_y+ xiuzheng);
-
-                if(ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
-                    xxx=0-xxx;
-                }
-
-                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
-                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
-
-
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
-                {
-
-                    if (lastEsrID == i)
-                    {
-                        lastEsrCount++;
-                    }
-                    else
-                    {
-                        lastEsrCount = 0;
-                    }
-
-                    if(yyy>50 ){
-                        if (lastEsrCount >=200)
-                        {
-                            return i;
-                        }
-                    }
-                    else if (lastEsrCount >= 1)
-                    {
-                        return i;
-                    }
-
-                    lastEsrID = i;
-                }
-            }
-    }
-    return -1;
-}
-
-
-
-
-
-//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
-//    bool isRemove = false;
-
-//    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-//    {
-
-//        for (int i = 0; i < 64; i++)
-//            if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
-//            {
-//                double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
-//                double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;
-
-//                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
-//                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
-
-
-//                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-//                {
-
-//                    if (lastEsrID == i)
-//                    {
-//                        lastEsrCount++;
-//                    }
-//                    else
-//                    {
-//                        lastEsrCount = 0;
-//                    }
-
-//                    if(yyy>50 ){
-//                        if (lastEsrCount >=200)
-//                        {
-//                            return i;
-//                        }
-//                    }
-//                    else if (lastEsrCount >= 3)
-//                    {
-//                        return i;
-//                    }
-
-//                    lastEsrID = i;
-//                }
-//            }
-//    }
-//    return -1;
-//}
-
-
-
-int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        for (int i = 0; i < 64; i++)
-            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
-            {
-                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-                double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
-
-                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-                {
-
-                    if (lastEsrIDAvoid == i)
-                    {
-                        lastEsrCountAvoid++;
-                    }
-                    else
-                    {
-                        lastEsrCountAvoid = 0;
-                    }
-
-                    if (lastEsrCountAvoid >= 6)
-                    {
-                        return i;
-                    }
-
-                    lastEsrIDAvoid = i;
-                }
-            }
-    }
-    return -1;
-}
-
-
-
-
-
-
-
-//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
-//	double obsSpeed = 0 - realSecSpeed;
-//	double minDis = iv::MaxValue;
-//	for (int i = 0; i < esrRadars.size(); i++)
-//		if ((esrRadars[i].nomal_y) != 0)
-//		{
-//			double xxx = esrRadars[i].nomal_x + Esr_Offset;
-//			double yyy = esrRadars[i].nomal_y;
-//
-//			if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-//			{
-//				double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-//				if (tmpDis < minDis)
-//				{
-//					minDis = tmpDis;
-//					obsSpeed = esrRadars[i].speed_y;
-//				}
-//			}
-//		}
-//
-//	return obsSpeed;
-//
-//
-//}
-
-
-
-
-double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
-    double obsSpeed = 0 - realSecSpeed;
-    double minDis = iv::MaxValue;
-    for (int i = 0; i < 64; i++)
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
-        {
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
-
-            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-            {
-                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-                if (tmpDis < minDis)
-                {
-                    minDis = tmpDis;
-                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
-                }
-            }
-        }
-
-    return obsSpeed;
-}
-
-
-
-
-double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-
-    double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-
-    if (gpsTrace[0].v1 == 1)
-    {
-        KEang = 10; KEPos = 8;
-        if (realSpeed > 60) KEang = 5;
-    }
-    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
-    {
-        KEang = 18; KEPos = 50; PreviewDistance = 3;
-    }
-    else if (gpsTrace[0].v1 == 7)
-    {
-        KEang = 20; KEPos = 50; PreviewDistance = 4;
-    }
-
-
-    double sumdis = 0;
-    int gpsIndex = 0;
-    std::vector<Point2D> farTrace;
-
-
-
-
-
-
-    for (int i = 1; i < gpsTrace.size() - 1; i++)
-    {
-        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
-        if (sumdis > PreviewDistance)
-        {
-            gpsIndex = i;
-            break;
-        }
-    }
-
-    if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
-    {
-        gpsIndex = DecideGps00().gpsLineParkIndex;
-    }
-
-
-
-    EPos = gpsTrace[gpsIndex].x + avoidX;
-
-    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
-        farTrace.push_back(gpsTrace[gpsIndex]);
-    }
-    if (farTrace.size() == 0) {
-        EAng = 0;
-    }
-    else {
-        EAng = getAvoidAveDef(farTrace, avoidX);
-    }
-
-    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-
-    return ang;
-}
-
-
-std::vector<iv::GPSData>   iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {
-
-    vector<vector<iv::GPSData>> maps;
-    vector<iv::GPSData> gpsMapLineBeside;
-    int sizeN = gpsMapLine.size();
-    for (int i = 1; i < sizeN; i++)
-    {
-        iv::GPSData gpsData(new GPS_INS);
-        double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
-        double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
-        double lng = ServiceCarStatus.location->ins_heading_angle;
-
-
-        double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
-        double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
-        double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
-        double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
-
-        //	memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));
-
-        gpsData->speed_mode = gpsMapLine[i]->speed_mode;
-        gpsData->gps_x = x0 + k1 * avoidX;
-        gpsData->gps_y = y0 + k2 * avoidX;
-        gpsMapLineBeside.push_back(gpsData);
-
-    }
-    return gpsMapLineBeside;
-
-}
-
-
-
-//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {
-
-//    double ang = 0;
-//    double EPos = 0, EAng = 0;
-
-// //   double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
-//       double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;
-
-// //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-//    double PreviewDistance;//预瞄距离
-//    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-
-
-
-
-
-////    if (realSpeed > 40)	KEang = 10; KEpos = 8;
-////        if (realSpeed > 50) KEang = 5;
-
-
-
-
-
-//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
-//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
-//double a = ServiceCarStatus.Lane.curvature;
-//double b = ServiceCarStatus.Lane.heading;
-//double c = (c1+c2)*0.5;
-//double x= PreviewDistance;
-//double y;
-
-
-//y=a*x*x+b*x+c;
-
-//   // EPos = y;
-//EPos=c;
-
-
-//  //  EAng=atan(2*a*x+b) / PI * 180;
-//    EAng=ServiceCarStatus.Lane.yaw;
-//        ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-//        lastEA = EAng;
-//        lastEP = EPos;
-
-
-//           std::cout << "\nEPos:%f\n" << EPos << std::endl;
-//            std::cout << "\nEAng:%f\n" << EAng << std::endl;
-
-
-//        if (ang > angleLimit) {
-//            ang = angleLimit;
-//        }
-//        else if (ang < -angleLimit) {
-//            ang = -angleLimit;
-//        }
-//        if (lastAng != iv::MaxValue) {
-//            ang = 0.2 * lastAng + 0.8 * ang;
-//            //ODS("lastAng：%d\n", lastAng);
-//        }
-//        lastAng = ang;
-//        return ang;
-//    }
-
-
-
-double IEPos = 0, IEang = 0;
-
-
-double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-    double Curve=0;
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-    double KCurve=120;
-    double KIEPos = 0, KIEang = 0;
-    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-    double PreviewDistance;//预瞄距离
-
-    int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
-    int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
-    int conf =min(confL,confR);
-
-    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-
-
-
-
-
-    if (realSpeed > 40)	KEang = 10; KEPos = 8;
-    if (realSpeed > 50) KEang = 5;
-
-    KEPos = 20;
-    KEang = 200;
-    //KEang = 15;
-
-    double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
-    double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
-    double a = ServiceCarStatus.Lane.curvature;
-    double b = ServiceCarStatus.Lane.heading;
-    double c = (c1+c2)*0.5;
-    double yaw= ServiceCarStatus.Lane.yaw;
-
-    double x= PreviewDistance;
-    double y;
-
-
-    y=c-(a*x*x+b*x);
-    double difa=0-(atan(2*a*x+b) / PI * 180);
-    Curve=0-a;
-
-    //EAng=difa;
-    //EPos=y;
-    EAng= 0-b;
-    EPos = c;
-    DEang = 10;
-    DEPos = 20;
-    //DEang = 20;
-    //DEPos = 10;
-
-    IEang = EAng+0.7*IEang;
-    IEPos = EPos+0.7*IEPos;
-    KIEang = 0;
-    //KIEang = 0.5;
-    KIEPos =2;
-
-
-
-    if(abs(confL)>=2&&abs(confR)>=2){
-        //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
-        ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
-    }else{
-        ang=lastAng;
-    }
-    //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-    return ang;
-}
-
-
-
-double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
-
-    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
-    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-
-
-
-    double x_1 = pt.x;
-    double y_1 = pt.y;
-    double angle_1 = getQieXianAngle(nowGps,aimGps);
-    double x_2 = 0.0, y_2 = 0.0;
-    double steering_angle;
-    double l = 2.950;
-    double r =6;
-    double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
-    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
-    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
-    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
-    double g_1 = tan(angle_1);
-    double car_pos[3] = { x_1,y_1,g_1 };
-    double parking_pos[2] = { x_2,y_2 };
-    double g_3;
-    double t[4][2];
-    double p[4];
-    double  s1, s2; //切点与车起始位置的距离
-    double  min;
-    int  min_i;
-
-    //g_3 = 0 - 0.5775;
-    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
-    //交点
-    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
-    y_3 = y_1 - g_1 * x_1;
-    //圆心1
-    x_o_1 = r;
-    y_o_1 = g_3 * r + y_3;
-    //圆形1的切点1
-    x_t_1 = 0.0;
-    y_t_1 = g_3 * r + y_3;
-    //圆形1的切点2
-    if (g_1 == 0)
-    {
-        x_t_2 = r;
-        y_t_2 = y_1 - g_1 * x_1;
-    }
-    else
-    {
-        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
-        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;
-
-    }
-    //圆心2
-    x_o_2 = 0 - r;
-    y_o_2 = y_3 - g_3 * r;
-    //圆形2的切点1
-    x_t_3 = 0;
-    y_t_3 = y_3 - g_3 * r;
-    //圆形2的切点2
-    if (g_1 == 0)
-    {
-        x_t_4 = 0 - r;
-        y_t_4 = y_1 - g_1 * x_1;
-    }
-    else
-    {
-        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
-        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;
-
-    }
-    t[0][0] = x_t_1;
-    t[0][1] = y_t_1;
-    t[1][0] = x_t_2;
-    t[1][1] = y_t_2;
-    t[2][0] = x_t_3;
-    t[2][1] = y_t_3;
-    t[3][0] = x_t_4;
-    t[3][1] = y_t_4;
-    for (int i = 0; i < 4; i++)
-    {
-
-        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
-
-    }
-    min = p[0];
-    min_i = 0;
-    for (int i = 1; i < 4; i++)
-    {
-
-        if (p[i] < min)
-        {
-            min = p[i]; min_i = i;
-        }
-    }
-    if (min_i < 2)
-    {
-        x_o = x_o_1;
-        y_o = y_o_1;
-        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
-        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
-        if (s1 < s2)
-        {
-            x_t_n = x_t_1;
-            y_t_n = y_t_1;
-            x_t_f = x_t_2;
-            y_t_f = y_t_2;
-        }
-        else
-        {
-            x_t_n = x_t_2;
-            y_t_n = y_t_2;
-            x_t_f = x_t_1;
-            y_t_f = y_t_1;
-
-        }
-    }
-    else
-    {
-        x_o = x_o_2;
-        y_o = y_o_2;
-        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
-        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
-
-        if (s1 < s2)
-        {
-
-            x_t_n = x_t_3;
-            y_t_n = y_t_3;
-            x_t_f = x_t_4;
-            y_t_f = y_t_4;
-        }
-        else
-        {
-            x_t_n = x_t_4;
-            y_t_n = y_t_4;
-            x_t_f = x_t_3;
-            y_t_f = y_t_3;
-        }
-
-
-
-
-    }
-    steering_angle = atan2(l, r);
-
-    if (x_t_n < 0)
-    {
-        steering_angle = 0 - steering_angle;
-    }
-
-    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
-    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
-    bocheAngle = steering_angle*180/PI;
-
-    cout << "近切点：x_t_n=" << x_t_n << endl;
-    cout << "近切点：y_t_n=" << y_t_n << endl;
-    cout << "远切点：x_t_f=" << x_t_f << endl;
-    cout << "远切点：y_t_f=" << y_t_f << endl;
-    cout << "航向角：" << steering_angle << endl;
-
-
-    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
-    //        return 1;
-    //    }
-    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
-    double disA = GetDistance(aimGps,nowGps);
-    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
-        return 1;
-    }
-
-    return 0;
-
-}
-
-
-
-//返回垂直平分线的斜率
-double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
-    double angl, x_3, angle_3;
-    if (tan(angle_1 == 0))
-    {
-        if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
-        {
-            angle_3 = 0 - 1;
-        }
-        else
-        {
-            angle_3 = 1;
-        }
-    }
-    else
-    {
-        x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
-        angl = tan(angle_1);//车所在直线的斜率
-        if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
-        {
-            if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
-            {
-                if (angl < 0)
-                {
-                    angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-                else
-                {
-                    angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-
-
-
-            }
-
-        }
-        else//第二象限
-        {
-            if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
-            {
-                if (angl < 0)
-                {
-                    angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-                else
-                {
-                    angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-
-            }
-        }
-    }
-
-    return angle_3;
-
-}
-
-
-
-double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
-    double heading = nowGps.ins_heading_angle *PI/180;
-    double x1 = nowGps.gps_x;
-    double y1 = nowGps.gps_y;
-    if (heading<=PI*0.5)
-    {
-        heading = 0.5*PI - heading;
-    }
-    else if (heading>PI*0.5 && heading<=PI*1.5) {
-        heading = 1.5*PI - heading;
-    }
-    else if (heading>PI*1.5) {
-        heading = 2.5*PI - heading;
-    }
-    double k1 = tan(heading);
-    double x = x1+10;
-    double y = k1 * x + y1 - (k1 * x1);
-    Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-    Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
-    double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
-    double angle = atan(abs(xielv));
-    if (xielv<0)
-    {
-        angle = PI - angle;
-    }
-    return angle;
-
-}
-
-
-/*
-  chuizhicheweiboche
-  */
-
-
-int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
-{
-
-
-
-    double l=2.95;//轴距
-    double x_0 = 0, y_0 = 0.5;
-    double x_1, y_1;//车起点坐标
-    double ange1;//车航向角弧度
-    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
-    double real_rad;;//另一条直线的航向角弧度
-    double angle_3;//垂直平分线弧度
-    double x_3, y_3;//垂直平分线交点
-    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
-    double x_o_1, y_o_1;//圆形1坐标
-    double x_o_2, y_o_2;//圆形2坐标
-    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
-    double min_rad;
-    double R_M; //后轴中点的转弯半径
-    double steering_angle;
-
-
-
-
-    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
-    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-    x_1=pt.x;
-    y_1=pt.y;
-    ange1=getQieXianAngle(nowGps,aimGps);
-
-    min_rad_zhuanxiang(&R_M , &min_rad);
-    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
-    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
-    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
-    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
-    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
-                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
-    steering_angle = atan2(l, R_M);
-    x_4 = 0.5;
-    y_4 = 0;
-    //for (int i = 0; i < 4; i++)
-    //{
-    //for (int j = 0; j < 2; j++)
-    //{
-    //	cout << t[i][j] << endl;
-    //}
-    //}
-    //cout << "min_rad:" << min_rad<< endl;
-    //cout << "jiaodian：x=" << x_3 << endl;
-    //cout << "jiaodian：y=" << y_3 << endl;
-    // cout << "R-M：" << R_M << endl;
-    cout << "x_0:" << x_0 << endl;
-    cout << "y_0:" << y_0 << endl;
-    cout << "x_2:" << x_2 << endl;
-    cout << "y_2:" << y_2 << endl;
-    cout << "近切点：x_t_n="<< x_t_n << endl;
-    cout << "近切点：y_t_n=" << y_t_n << endl;
-    cout << "远切点：x_t_f=" << x_t_f << endl;
-    cout << "远切点：y_t_f=" << y_t_f << endl;
-    //cout << "航向角：" << steering_angle << endl;
-    //cout << "圆心1横坐标=" << x_o_1 << endl;
-    //cout << "圆心1纵坐标=" << y_o_1 << endl;
-    //cout << "圆心2横坐标=" << x_o_2 << endl;
-    //cout << "圆心2纵坐标=" << y_o_2 << endl;
-    //cout << "平分线弧度=" << angle_3 << endl;
-    //cout << " min_rad=" << min_rad << endl;
-    //cout << " real_rad=" << real_rad << endl;
-    //   system("PAUSE");
-
-
-
-    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
-    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
-    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
-    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
-    dBocheAngle = steering_angle*180/PI;
-
-    double disA = GetDistance(aimGps,nowGps);
-
-    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
-        return 1;
-    }
-    return 0;
-
-}
-
-
-double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
-    double L_c = 4.749;//车长
-    double rad_1;
-    double rad_2;
-    double L_k = 1.931;//车宽
-    double L = 2.95;//轴距
-    double L_f =1.2 ;//前悬
-    double L_r =0.7 ;//后悬
-    double R_min =6.5 ;//最小转弯半径
-    *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
-    //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
-    //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
-    *min_rad = 45 * PI / 180;// rad_1 - rad_2;
-    return 0;
-}
-
-
-double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {
-
-    if (x_1 > 0 && y_1 > 0)
-    {
-        *real_rad = PI*0.5 - min_rad;
-        *x_2 = R_M - R_M*cos(min_rad);
-        *y_2 = R_M*sin(min_rad) + 0.5;
-    }
-    else
-    {
-        *real_rad = PI*0.5 + min_rad;
-        *x_2 = R_M*cos(min_rad) - R_M;
-        *y_2 = R_M*sin(min_rad) + 0.5;
-    }
-    return 0;
-
-}
-
-
-double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
-    double b1, b2;
-    double k1, k2;
-    if (ange1!=(PI*0.5))
-    {
-        k1 = tan(ange1);
-        b1 = y_1 - k1*x_1;
-        k2 = tan(real_rad);
-        b2 = y_2 - k2*x_2;
-        *x_3 = (b2 - b1) / (k1 - k2);
-        *y_3 = k2*(*x_3) + b2;
-    }
-    else
-    {
-        k2 = tan(real_rad);
-        b2 = y_2 - k2*x_2;
-        *x_3 = x_1;
-        *y_3 = k2*(*x_3) + b2;
-    }
-    return 0;
-}
-
-
-double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
-    double k1, k2;
-    double  angle_j;
-    k2 = tan(real_rad);
-    if (ange1 != (PI*0.5))
-    {
-        k1 = tan(ange1);
-        angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角
-
-        if (x_1 > 0 && y_1 > 0)
-        {
-            *angle_3 = angle_j*0.5 - min_rad + PI;
-        }
-        else
-        {
-            *angle_3 = min_rad - angle_j*0.5;
-        }
-    }
-    else
-    {
-        angle_j = min_rad;//两直线夹角
-        if (x_1 > 0 && y_1 > 0)
-        {
-            *angle_3 = angle_j*0.5 - min_rad + PI;
-        }
-        else
-        {
-            *angle_3 = min_rad - angle_j*0.5;
-        }
-    }
-    return 0;
-}
-
-
-double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
-                                        double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
-    double b2, b3, k2, k3;
-    b2 = y_2 - tan(real_rad)*x_2;
-    b3 = y_3 - tan(angle_3)*x_3;
-    k2 = tan(real_rad);
-    k3 = tan(angle_3);
-    *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
-    *y_o_1 = k3*(*x_o_1) + b3;
-
-    *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
-    *y_o_2 = k3*(*x_o_2) + b3;
-    return 0;
-}
-
-
-double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
-                                                double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
-                                                double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
-{
-    double x_o, y_o;
-    double b2, b3, k1, k2, k3;
-    //double car_pos[3] = { x_1,y_1,k1 };
-    double parking_pos[2] = { x_2,y_2 };
-    //double t[4][2];
-    double p[4];
-    double  s1, s2; //切点与车起始位置的距离
-    double  min;
-    int  min_i;
-    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
-    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
-    double t[4][2];
-    k1 = tan(ange1);
-    b2 = y_2 - tan(real_rad)*x_2;
-    b3 = y_3 - tan(real_rad)*x_3;
-    k2 = tan(real_rad);//另一条直线斜率
-    k3 = tan(angle_3);//垂直平分线斜率
-    //圆心1和2切点*********************************************
-    if (x_1 > 0 && y_1 > 0)//第一象限
-    {
-        if (ange1 == (PI*0.5))
-        {
-            x_t_1 = x_1;
-            y_t_1 = y_o_1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            x_t_3 = x_1;
-            y_t_3 = y_o_2;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-
-        }
-        else
-        {
-            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-        }
-    }
-    else
-    {
-        if (ange1 == 0)
-        {
-            x_t_1 = 0 - x_1;
-            y_t_1 = y_o_1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            x_t_3 = 0 - x_1;
-            y_t_3 = y_o_2;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-        }
-        else
-        {
-            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-        }
-
-    }
-
-    //圆心1和2切点*********************************************
-
-    t[0][0] = x_t_1;
-    t[0][1] = y_t_1;
-    t[1][0] = x_t_2;
-    t[1][1] = y_t_2;
-    t[2][0] = x_t_3;
-    t[2][1] = y_t_3;
-    t[3][0] = x_t_4;
-    t[3][1] = y_t_4;
-    for (int i = 0; i < 4; i++)
-    {
-
-        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
-
-    }
-    min = p[0];
-    min_i = 0;
-    for (int i = 1; i < 4; i++)
-    {
-
-        if (p[i] < min)
-        {
-            min = p[i]; min_i = i;
-        }
-    }
-    if (min_i < 2)
-    {
-        x_o = x_o_1;
-        y_o = y_o_1;
-        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
-        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
-        if (s1 < s2)
-        {
-            *x_t_n = x_t_1;
-            *y_t_n = y_t_1;
-            *x_t_f = x_t_2;
-            *y_t_f = y_t_2;
-        }
-        else
-        {
-            *x_t_n = x_t_2;
-            *y_t_n = y_t_2;
-            *x_t_f = x_t_1;
-            *y_t_f = y_t_1;
-
-        }
-    }
-    else
-    {
-        x_o = x_o_2;
-        y_o = y_o_2;
-        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
-        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
-
-        if (s1 < s2)
-        {
-
-            *x_t_n = x_t_3;
-            *y_t_n = y_t_3;
-            *x_t_f = x_t_4;
-            *y_t_f = y_t_4;
-        }
-        else
-        {
-            *x_t_n = x_t_4;
-            *y_t_n = y_t_4;
-            *x_t_f = x_t_3;
-            *y_t_f = y_t_3;
-        }
-
-
-
-    }
-
-    return 0;
-
-}
-
-
-int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
-{
-    int index = -1;
-    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = gpsMap.size() - 1;
-    float minDis=20;
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < minDis)
-        {
-            index = i;
-            minDis=tmpdis;
-        }
-    }
-    return index;
-}
-
-
-double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps) {
-    double obsSpeed = 0 - realSecSpeed;
-    double minDis = iv::MaxValue;
-    FrenetPoint esr_obs_F_point;
-    for (int i = 0; i < 64; i++)
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
-        {
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
-
-            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-            {
-                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-                if (tmpDis < minDis)
-                {
-                    minDis = tmpDis;
-//                    esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
-                    esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
-//                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
-                    double speedx=ServiceCarStatus.obs_radar[i].speed_x;  //障碍物相对于车辆x轴的速度
-                    double speedy=ServiceCarStatus.obs_radar[i].speed_y;  //障碍物相对于车辆y轴的速度
-                    double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
-                    //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
-                    //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
-                    double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);
-
-                    obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
-                }
-            }
-        }
-
-    return obsSpeed;
-}
-
-
-int iv::decition::Compute00::getEsrIndexByFrenet(std::vector<Point2D> gpsTrace, FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps){
-    double minDistance = numeric_limits<double>::max();
-    int minDis_index=-1;
-
-    for(int i=0; i<64; ++i){
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
-            //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
-
-            //将毫米波障碍物位置转换到frenet坐标系下
-//            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
-            esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
-
-            //如果障碍物与道路的横向距离d<=3.0*Veh_Width / 4.0，则认为道路上有障碍物。
-            //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
-            //minDistance、minDis_index用来统计最近的障碍物信息。
-            if(abs(esrObsPoint.d)<=(3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs)){
-                if(esrObsPoint.s<minDistance){
-                    minDistance = esrObsPoint.s;
-                    minDis_index = i;
-                }
-            }
-        }
-    }
-    return minDis_index;
-}
-
-
-
-
-std::vector<std::vector<iv::GPSData>> gmapsL;
-std::vector<std::vector<iv::GPSData>> gmapsR;
+#include <decition/adc_tools/compute_00.h>
+#include <decition/decide_gps_00.h>
+#include <decition/adc_tools/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/adc_tools/transfer.h>
+#include <common/constants.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <control/can.h>
+using namespace std;
+
+#define PI (3.1415926535897932384626433832795)
+
+iv::decition::Compute00::Compute00() {
+
+}
+iv::decition::Compute00::~Compute00() {
+
+}
+
+
+double iv::decition::Compute00::angleLimit = 700;
+double iv::decition::Compute00::lastEA = 0;
+double iv::decition::Compute00::lastEP = 0;
+double iv::decition::Compute00::decision_Angle = 0;
+double iv::decition::Compute00::lastAng = 0;
+int iv::decition::Compute00::lastEsrID = -1;
+int  iv::decition::Compute00::lastEsrCount = 0;
+int iv::decition::Compute00::lastEsrIDAvoid = -1;
+int  iv::decition::Compute00::lastEsrCountAvoid = 0;
+
+iv::GPS_INS  iv::decition::Compute00::nearTpoint;
+iv::GPS_INS  iv::decition::Compute00::farTpoint;
+double  iv::decition::Compute00::bocheAngle;
+
+
+
+iv::GPS_INS  iv::decition::Compute00::dTpoint0;
+iv::GPS_INS  iv::decition::Compute00::dTpoint1;
+iv::GPS_INS  iv::decition::Compute00::dTpoint2;
+iv::GPS_INS  iv::decition::Compute00::dTpoint3;
+double  iv::decition::Compute00::dBocheAngle;
+
+
+std::vector<int> lastEsrIdVector;
+std::vector<int> lastEsrCountVector;
+
+
+
+int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
+{
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+    static double FrontTotalFive=0,FrontAveFive=0,BackAveFive=0,BackTotalFive=0;
+    static int FrontCount=0,BackCount=0;
+    static int CurrentIndex=0,MarkingIndex=0,MarkingCount=0;
+    int MarkedIndex=0,CurveContinue=0;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        (*gpsMap[i]).roadMode=5;
+    }
+    for (int j = startIndex; j < (endIndex-10); j+=10)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        for(int front_k=i;front_k<i+5;front_k++)
+        {
+            if(fabs(((*gpsMap[front_k]).ins_heading_angle)-((*gpsMap[i]).ins_heading_angle))<10)
+            {
+                   FrontTotalFive+=(*gpsMap[front_k]).ins_heading_angle;
+                   FrontCount++;
+            }
+
+        }
+        i+=5;
+        FrontAveFive=FrontTotalFive/FrontCount;
+        FrontTotalFive=0;
+        FrontCount=0;
+        for(int back_k=i;back_k<i+5;back_k++)
+        {
+            if(fabs((*gpsMap[back_k]).ins_heading_angle-(*gpsMap[i]).ins_heading_angle)<10)
+            {
+                   BackTotalFive+=(*gpsMap[back_k]).ins_heading_angle;
+                   BackCount++;
+            }
+
+        }
+        i+=5;
+        CurrentIndex=i-1;
+        BackAveFive=BackTotalFive/BackCount;
+        BackTotalFive=0;
+        BackCount=0;
+        if(fabs(FrontAveFive-BackAveFive)<50)
+        {
+                   if(fabs(FrontAveFive-BackAveFive)>3)
+                   {
+                        CurveContinue+=1;
+                        if(CurveContinue>=2)
+                        {
+                            MarkingCount=0;
+                            for(MarkingIndex=CurrentIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
+                            {
+
+                                if(MarkingCount<100)
+                                {
+                                     if((BackAveFive-FrontAveFive)<3)
+                                     {
+                                           (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
+                                     }
+                                     else if((BackAveFive-FrontAveFive)>3)
+                                     {
+                                           (*gpsMap[MarkingIndex]).roadMode=15;
+                                     }
+
+                                }
+                                else if((MarkingCount>=100)&&(MarkingCount<150))
+                                {
+                                     (*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米,1
+                                }
+                                else if((MarkingCount>=150)&&(MarkingCount<320))
+                                {
+                                     (*gpsMap[MarkingIndex]).roadMode=5;   //低速，20米,5
+                                }
+                                else if((MarkingCount>=320)&&(MarkingCount<620))
+                                {
+                                     (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
+                                }
+                                else if(MarkingCount>=620)
+                                {
+                                     (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
+                                }
+
+                                MarkingCount++;
+                            }
+                            MarkedIndex=CurrentIndex;
+                        }
+                   }
+                   else if(fabs(FrontAveFive-BackAveFive)<=3)
+                   {
+                        CurveContinue=0;
+                   }
+        }
+        FrontAveFive=0;
+        BackAveFive=0;
+    }
+
+    if(MarkedIndex<endIndex)
+    {
+        MarkingCount=0;
+        for(MarkingIndex=endIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
+        {
+
+            if(MarkingCount<100)
+            {
+                if((BackAveFive-FrontAveFive)<3)
+                {
+                      (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
+                }
+                else if((BackAveFive-FrontAveFive)>3)
+                {
+                      (*gpsMap[MarkingIndex]).roadMode=15;
+                }
+            }
+            else if((MarkingCount>=100)&&(MarkingCount<150))
+            {
+                 (*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米
+            }
+            else if((MarkingCount>=150)&&(MarkingCount<320))
+            {
+                 (*gpsMap[MarkingIndex]).roadMode=5;   //低速，30米
+            }
+            else if((MarkingCount>=320)&&(MarkingCount<620))
+            {
+                 (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
+            }
+            else if(MarkingCount>=620)
+            {
+                 (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
+            }
+
+            MarkingCount++;
+        }
+    }
+
+
+    return 1;
+}
+
+
+
+//首次找点
+
+int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
+{
+    int index = -1;
+    //	DecideGps00().minDis = iv::MaxValue;
+    float minDis = 10;
+    double maxAng = iv::MaxValue;
+
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
+                )
+        {
+            index = i;
+            minDis = tmpdis;
+            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
+            maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+        }
+    }
+
+    DecideGps00().maxAngle=maxAng;
+    DecideGps00().minDis=minDis;
+    return index;
+}
+
+//search pathpoint
+int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
+{
+    int index = -1;
+    float minDis = 10;
+    double maxAng = iv::MaxValue;
+    int map_size=gpsMap.size();
+    int preDistance=max(100,(int)(rp.speed*10));
+        preDistance=min(500,preDistance);
+
+    int startIndex = max((int)(lastIndex - 100),(int)(lastIndex-map_size));     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = min((int)(lastIndex + preDistance ),(int)(lastIndex+map_size));
+
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + map_size) % map_size;
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
+            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
+            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
+            )
+        {
+            index = i;
+            minDis = tmpdis;
+            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
+            maxAng = min(maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+        }
+    }
+
+
+    DecideGps00().maxAngle=maxAng;
+    DecideGps00().minDis=minDis;
+    return index;
+}
+
+
+double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
+{
+    double sum_x = 0;
+    double sum_y = 0;
+
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+
+
+    return atan(average_x / average_y) / PI * 180;
+}
+
+
+
+double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
+{
+    double sum_x = 0;
+    double sum_y = 0;
+
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+
+
+    return atan(average_x + avoidX / average_y) / PI * 180;
+}
+
+
+
+
+
+
+double iv::decition::Compute00::getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    //   double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1;  // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+
+    if(transferPieriod&& !transferPieriod2){
+        DEang = 200;
+        DEPos = 150;
+    }
+
+    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+    double PreviewDistance;//预瞄距离
+    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+    if(changeRoad ||transferPieriod){
+        PreviewDistance=PreviewDistance+avoidX;
+    }
+    if(realSpeed <15){
+        PreviewDistance = max(4.0, realSpeed *0.4) ;
+    }
+
+    if (gpsTrace[0].v1 == 1)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
+    {
+        KEang = 18; KEPos = 50; PreviewDistance = 3;
+    }
+    else if (gpsTrace[0].v1 == 7)
+    {
+        KEang = 20; KEPos = 50; PreviewDistance = 4;
+    }
+
+
+    if (realSpeed > 40)	KEang = 10; KEPos = 8;
+    if (realSpeed > 50) KEang = 5;
+
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+
+
+
+
+
+    for (int i = 1; i < gpsTrace.size() - 1; i++)
+    {
+        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+
+
+
+
+    EPos = gpsTrace[gpsIndex].x;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
+        farTrace.push_back(gpsTrace[gpsIndex]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAveDef(farTrace);
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+    return ang;
+}
+
+
+
+
+int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
+{
+    int index = 1;
+    double sumdis = 0;
+    while (index < gpsTrace.size() && sumdis < realSpeed)
+        sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);
+
+    if (index == gpsTrace.size())
+        return index - 1;
+
+    if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
+        index--;
+    return index;
+}
+
+iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
+    }
+
+
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + Veh_Width / 2 * cos(anglevalue + PI / 2),
+                       carFronty + Veh_Width / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + Veh_Width / 2 * cos(anglevalue - PI / 2),
+                        carFronty + Veh_Width / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
+                int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                isRemove = true;
+                //		DecideGps00().lidarDistance = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistance = obsPoint.y;
+    return obsPoint;
+}
+
+//1220
+iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=0-ServiceCarStatus.msysparam.rearLidarGpsXiuzheng;
+    }
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue + PI / 2),
+                       carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue - PI / 2),
+                        carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y<(0-ServiceCarStatus.msysparam.rearGpsXiuzheng) )
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+
+                dx=grx-dx;//1227
+
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+
+                dx=grx-dx;//1227
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                //		DecideGps00().lidarDistance = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistance = obsPoint.y;
+    return obsPoint;
+}
+
+
+iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        //1127 fanwei xiuzheng
+        float buchang=0;
+        Point2D ptLeft(carFrontx + (Veh_Width+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
+                       carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx +  (Veh_Width+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
+                        carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
+                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                DecideGps00().lidarDistanceAvoid = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistanceAvoid = obsPoint.y;
+
+    return obsPoint;
+}
+
+
+
+
+//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
+//	bool isRemove = false;
+//
+//	for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+//	{
+//
+//		for (int i = 0; i < esrRadars.size(); i++)
+//			if ((esrRadars[i].nomal_y) != 0)
+//			{
+//				double xxx = esrRadars[i].nomal_x + Esr_Offset;
+//				double yyy = esrRadars[i].nomal_y;
+//
+//				if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//				{
+//
+//					if (lastEsrID == (esrRadars[i]).esr_ID)
+//					{
+//						lastEsrCount++;
+//					}
+//					else
+//					{
+//						lastEsrCount = 0;
+//					}
+//
+//					if (lastEsrCount >= 3)
+//					{
+//						return i;
+//					}
+//
+//					lastEsrID = (esrRadars[i]).esr_ID;
+//				}
+//			}
+//	}
+//	return -1;
+//}
+
+
+
+
+int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint) {
+    bool isRemove = false;
+
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=ServiceCarStatus.msysparam.radarGpsXiuzheng;
+    }
+
+    float fxiuzhengCs = DecideGps00().xiuzhengCs;
+    int nsize = gpsTrace.size();
+    for (int j = 1; j < nsize - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
+            {
+                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+                double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ xiuzheng;
+
+                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+//优化
+//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*Veh_Width / 2.0+DecideGps00().xiuzhengCs)){
+//                    *esrPathpoint = j;
+//                    return i;
+//                }
+
+
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrID == i)
+                    {
+                        lastEsrCount++;
+                    }
+                    else
+                    {
+                        lastEsrCount = 0;
+                    }
+
+                    if(yyy>50 ){
+                        if (lastEsrCount >=200)
+                        {
+                            return i;
+                        }
+                    }
+                    else if (lastEsrCount >= 1)
+                    {
+                        return i;
+                    }
+
+                    lastEsrID = i;
+                }
+            }
+    }
+    return -1;
+}
+
+
+
+
+int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
+    bool isRemove = false;
+
+    float xiuzheng = ServiceCarStatus.msysparam.rearRadarGpsXiuzheng;
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_rear_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_rear_radar[i].valid))
+            {
+                double xxx = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_x + Esr_Offset);
+                double yyy = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_y+ xiuzheng);
+
+                if(ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
+                    xxx=0-xxx;
+                }
+
+                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrID == i)
+                    {
+                        lastEsrCount++;
+                    }
+                    else
+                    {
+                        lastEsrCount = 0;
+                    }
+
+                    if(yyy>50 ){
+                        if (lastEsrCount >=200)
+                        {
+                            return i;
+                        }
+                    }
+                    else if (lastEsrCount >= 1)
+                    {
+                        return i;
+                    }
+
+                    lastEsrID = i;
+                }
+            }
+    }
+    return -1;
+}
+
+
+
+
+
+//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
+//    bool isRemove = false;
+
+//    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+//    {
+
+//        for (int i = 0; i < 64; i++)
+//            if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
+//            {
+//                double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
+//                double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+//                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+//                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+
+//                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//                {
+
+//                    if (lastEsrID == i)
+//                    {
+//                        lastEsrCount++;
+//                    }
+//                    else
+//                    {
+//                        lastEsrCount = 0;
+//                    }
+
+//                    if(yyy>50 ){
+//                        if (lastEsrCount >=200)
+//                        {
+//                            return i;
+//                        }
+//                    }
+//                    else if (lastEsrCount >= 3)
+//                    {
+//                        return i;
+//                    }
+
+//                    lastEsrID = i;
+//                }
+//            }
+//    }
+//    return -1;
+//}
+
+
+
+int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
+            {
+                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+                double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
+
+                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrIDAvoid == i)
+                    {
+                        lastEsrCountAvoid++;
+                    }
+                    else
+                    {
+                        lastEsrCountAvoid = 0;
+                    }
+
+                    if (lastEsrCountAvoid >= 6)
+                    {
+                        return i;
+                    }
+
+                    lastEsrIDAvoid = i;
+                }
+            }
+    }
+    return -1;
+}
+
+
+
+
+
+
+
+//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
+//	double obsSpeed = 0 - realSecSpeed;
+//	double minDis = iv::MaxValue;
+//	for (int i = 0; i < esrRadars.size(); i++)
+//		if ((esrRadars[i].nomal_y) != 0)
+//		{
+//			double xxx = esrRadars[i].nomal_x + Esr_Offset;
+//			double yyy = esrRadars[i].nomal_y;
+//
+//			if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+//			{
+//				double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+//				if (tmpDis < minDis)
+//				{
+//					minDis = tmpDis;
+//					obsSpeed = esrRadars[i].speed_y;
+//				}
+//			}
+//		}
+//
+//	return obsSpeed;
+//
+//
+//}
+
+
+
+
+double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
+    double obsSpeed = 0 - realSecSpeed;
+    double minDis = iv::MaxValue;
+    for (int i = 0; i < 64; i++)
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
+        {
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
+
+            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+            {
+                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+                if (tmpDis < minDis)
+                {
+                    minDis = tmpDis;
+                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
+                }
+            }
+        }
+
+    return obsSpeed;
+}
+
+
+
+
+double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+
+    double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+
+    if (gpsTrace[0].v1 == 1)
+    {
+        KEang = 10; KEPos = 8;
+        if (realSpeed > 60) KEang = 5;
+    }
+    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
+    {
+        KEang = 18; KEPos = 50; PreviewDistance = 3;
+    }
+    else if (gpsTrace[0].v1 == 7)
+    {
+        KEang = 20; KEPos = 50; PreviewDistance = 4;
+    }
+
+
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+
+
+
+
+
+    for (int i = 1; i < gpsTrace.size() - 1; i++)
+    {
+        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+
+    if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
+    {
+        gpsIndex = DecideGps00().gpsLineParkIndex;
+    }
+
+
+
+    EPos = gpsTrace[gpsIndex].x + avoidX;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
+        farTrace.push_back(gpsTrace[gpsIndex]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAvoidAveDef(farTrace, avoidX);
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+
+    return ang;
+}
+
+
+std::vector<iv::GPSData>   iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {
+
+    vector<vector<iv::GPSData>> maps;
+    vector<iv::GPSData> gpsMapLineBeside;
+    int sizeN = gpsMapLine.size();
+    for (int i = 1; i < sizeN; i++)
+    {
+        iv::GPSData gpsData(new GPS_INS);
+        double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
+        double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
+        double lng = ServiceCarStatus.location->ins_heading_angle;
+
+
+        double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
+        double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
+        double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
+        double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
+
+        //	memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));
+
+        gpsData->speed_mode = gpsMapLine[i]->speed_mode;
+        gpsData->gps_x = x0 + k1 * avoidX;
+        gpsData->gps_y = y0 + k2 * avoidX;
+        gpsMapLineBeside.push_back(gpsData);
+
+    }
+    return gpsMapLineBeside;
+
+}
+
+
+
+//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {
+
+//    double ang = 0;
+//    double EPos = 0, EAng = 0;
+
+// //   double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
+//       double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;
+
+// //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+//    double PreviewDistance;//预瞄距离
+//    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+
+
+
+
+////    if (realSpeed > 40)	KEang = 10; KEpos = 8;
+////        if (realSpeed > 50) KEang = 5;
+
+
+
+
+
+//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
+//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
+//double a = ServiceCarStatus.Lane.curvature;
+//double b = ServiceCarStatus.Lane.heading;
+//double c = (c1+c2)*0.5;
+//double x= PreviewDistance;
+//double y;
+
+
+//y=a*x*x+b*x+c;
+
+//   // EPos = y;
+//EPos=c;
+
+
+//  //  EAng=atan(2*a*x+b) / PI * 180;
+//    EAng=ServiceCarStatus.Lane.yaw;
+//        ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+//        lastEA = EAng;
+//        lastEP = EPos;
+
+
+//           std::cout << "\nEPos:%f\n" << EPos << std::endl;
+//            std::cout << "\nEAng:%f\n" << EAng << std::endl;
+
+
+//        if (ang > angleLimit) {
+//            ang = angleLimit;
+//        }
+//        else if (ang < -angleLimit) {
+//            ang = -angleLimit;
+//        }
+//        if (lastAng != iv::MaxValue) {
+//            ang = 0.2 * lastAng + 0.8 * ang;
+//            //ODS("lastAng：%d\n", lastAng);
+//        }
+//        lastAng = ang;
+//        return ang;
+//    }
+
+
+
+double IEPos = 0, IEang = 0;
+
+
+double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+    double Curve=0;
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+    double KCurve=120;
+    double KIEPos = 0, KIEang = 0;
+    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+    double PreviewDistance;//预瞄距离
+
+    int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
+    int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
+    int conf =min(confL,confR);
+
+    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+
+
+
+
+    if (realSpeed > 40)	KEang = 10; KEPos = 8;
+    if (realSpeed > 50) KEang = 5;
+
+    KEPos = 20;
+    KEang = 200;
+    //KEang = 15;
+
+    double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
+    double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
+    double a = ServiceCarStatus.Lane.curvature;
+    double b = ServiceCarStatus.Lane.heading;
+    double c = (c1+c2)*0.5;
+    double yaw= ServiceCarStatus.Lane.yaw;
+
+    double x= PreviewDistance;
+    double y;
+
+
+    y=c-(a*x*x+b*x);
+    double difa=0-(atan(2*a*x+b) / PI * 180);
+    Curve=0-a;
+
+    //EAng=difa;
+    //EPos=y;
+    EAng= 0-b;
+    EPos = c;
+    DEang = 10;
+    DEPos = 20;
+    //DEang = 20;
+    //DEPos = 10;
+
+    IEang = EAng+0.7*IEang;
+    IEPos = EPos+0.7*IEPos;
+    KIEang = 0;
+    //KIEang = 0.5;
+    KIEPos =2;
+
+
+
+    if(abs(confL)>=2&&abs(confR)>=2){
+        //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
+        ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
+    }else{
+        ang=lastAng;
+    }
+    //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+    return ang;
+}
+
+
+
+double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
+
+    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
+    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+
+
+
+    double x_1 = pt.x;
+    double y_1 = pt.y;
+    double angle_1 = getQieXianAngle(nowGps,aimGps);
+    double x_2 = 0.0, y_2 = 0.0;
+    double steering_angle;
+    double l = 2.950;
+    double r =6;
+    double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
+    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
+    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
+    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
+    double g_1 = tan(angle_1);
+    double car_pos[3] = { x_1,y_1,g_1 };
+    double parking_pos[2] = { x_2,y_2 };
+    double g_3;
+    double t[4][2];
+    double p[4];
+    double  s1, s2; //切点与车起始位置的距离
+    double  min;
+    int  min_i;
+
+    //g_3 = 0 - 0.5775;
+    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
+    //交点
+    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
+    y_3 = y_1 - g_1 * x_1;
+    //圆心1
+    x_o_1 = r;
+    y_o_1 = g_3 * r + y_3;
+    //圆形1的切点1
+    x_t_1 = 0.0;
+    y_t_1 = g_3 * r + y_3;
+    //圆形1的切点2
+    if (g_1 == 0)
+    {
+        x_t_2 = r;
+        y_t_2 = y_1 - g_1 * x_1;
+    }
+    else
+    {
+        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
+        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;
+
+    }
+    //圆心2
+    x_o_2 = 0 - r;
+    y_o_2 = y_3 - g_3 * r;
+    //圆形2的切点1
+    x_t_3 = 0;
+    y_t_3 = y_3 - g_3 * r;
+    //圆形2的切点2
+    if (g_1 == 0)
+    {
+        x_t_4 = 0 - r;
+        y_t_4 = y_1 - g_1 * x_1;
+    }
+    else
+    {
+        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
+        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;
+
+    }
+    t[0][0] = x_t_1;
+    t[0][1] = y_t_1;
+    t[1][0] = x_t_2;
+    t[1][1] = y_t_2;
+    t[2][0] = x_t_3;
+    t[2][1] = y_t_3;
+    t[3][0] = x_t_4;
+    t[3][1] = y_t_4;
+    for (int i = 0; i < 4; i++)
+    {
+
+        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
+
+    }
+    min = p[0];
+    min_i = 0;
+    for (int i = 1; i < 4; i++)
+    {
+
+        if (p[i] < min)
+        {
+            min = p[i]; min_i = i;
+        }
+    }
+    if (min_i < 2)
+    {
+        x_o = x_o_1;
+        y_o = y_o_1;
+        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
+        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
+        if (s1 < s2)
+        {
+            x_t_n = x_t_1;
+            y_t_n = y_t_1;
+            x_t_f = x_t_2;
+            y_t_f = y_t_2;
+        }
+        else
+        {
+            x_t_n = x_t_2;
+            y_t_n = y_t_2;
+            x_t_f = x_t_1;
+            y_t_f = y_t_1;
+
+        }
+    }
+    else
+    {
+        x_o = x_o_2;
+        y_o = y_o_2;
+        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
+        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
+
+        if (s1 < s2)
+        {
+
+            x_t_n = x_t_3;
+            y_t_n = y_t_3;
+            x_t_f = x_t_4;
+            y_t_f = y_t_4;
+        }
+        else
+        {
+            x_t_n = x_t_4;
+            y_t_n = y_t_4;
+            x_t_f = x_t_3;
+            y_t_f = y_t_3;
+        }
+
+
+
+
+    }
+    steering_angle = atan2(l, r);
+
+    if (x_t_n < 0)
+    {
+        steering_angle = 0 - steering_angle;
+    }
+
+    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
+    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
+    bocheAngle = steering_angle*180/PI;
+
+    cout << "近切点：x_t_n=" << x_t_n << endl;
+    cout << "近切点：y_t_n=" << y_t_n << endl;
+    cout << "远切点：x_t_f=" << x_t_f << endl;
+    cout << "远切点：y_t_f=" << y_t_f << endl;
+    cout << "航向角：" << steering_angle << endl;
+
+
+    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
+    //        return 1;
+    //    }
+    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
+    double disA = GetDistance(aimGps,nowGps);
+    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
+        return 1;
+    }
+
+    return 0;
+
+}
+
+
+
+//返回垂直平分线的斜率
+double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
+    double angl, x_3, angle_3;
+    if (tan(angle_1 == 0))
+    {
+        if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
+        {
+            angle_3 = 0 - 1;
+        }
+        else
+        {
+            angle_3 = 1;
+        }
+    }
+    else
+    {
+        x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
+        angl = tan(angle_1);//车所在直线的斜率
+        if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
+        {
+            if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
+            {
+                if (angl < 0)
+                {
+                    angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+                else
+                {
+                    angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+
+
+
+            }
+
+        }
+        else//第二象限
+        {
+            if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
+            {
+                if (angl < 0)
+                {
+                    angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+                else
+                {
+                    angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+
+            }
+        }
+    }
+
+    return angle_3;
+
+}
+
+
+
+double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
+    double heading = nowGps.ins_heading_angle *PI/180;
+    double x1 = nowGps.gps_x;
+    double y1 = nowGps.gps_y;
+    if (heading<=PI*0.5)
+    {
+        heading = 0.5*PI - heading;
+    }
+    else if (heading>PI*0.5 && heading<=PI*1.5) {
+        heading = 1.5*PI - heading;
+    }
+    else if (heading>PI*1.5) {
+        heading = 2.5*PI - heading;
+    }
+    double k1 = tan(heading);
+    double x = x1+10;
+    double y = k1 * x + y1 - (k1 * x1);
+    Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+    Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
+    double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
+    double angle = atan(abs(xielv));
+    if (xielv<0)
+    {
+        angle = PI - angle;
+    }
+    return angle;
+
+}
+
+
+/*
+  chuizhicheweiboche
+  */
+
+
+int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
+{
+
+
+
+    double l=2.95;//轴距
+    double x_0 = 0, y_0 = 0.5;
+    double x_1, y_1;//车起点坐标
+    double ange1;//车航向角弧度
+    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
+    double real_rad;;//另一条直线的航向角弧度
+    double angle_3;//垂直平分线弧度
+    double x_3, y_3;//垂直平分线交点
+    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
+    double x_o_1, y_o_1;//圆形1坐标
+    double x_o_2, y_o_2;//圆形2坐标
+    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
+    double min_rad;
+    double R_M; //后轴中点的转弯半径
+    double steering_angle;
+
+
+
+
+    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
+    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+    x_1=pt.x;
+    y_1=pt.y;
+    ange1=getQieXianAngle(nowGps,aimGps);
+
+    min_rad_zhuanxiang(&R_M , &min_rad);
+    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
+    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
+    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
+    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
+    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
+                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
+    steering_angle = atan2(l, R_M);
+    x_4 = 0.5;
+    y_4 = 0;
+    //for (int i = 0; i < 4; i++)
+    //{
+    //for (int j = 0; j < 2; j++)
+    //{
+    //	cout << t[i][j] << endl;
+    //}
+    //}
+    //cout << "min_rad:" << min_rad<< endl;
+    //cout << "jiaodian：x=" << x_3 << endl;
+    //cout << "jiaodian：y=" << y_3 << endl;
+    // cout << "R-M：" << R_M << endl;
+    cout << "x_0:" << x_0 << endl;
+    cout << "y_0:" << y_0 << endl;
+    cout << "x_2:" << x_2 << endl;
+    cout << "y_2:" << y_2 << endl;
+    cout << "近切点：x_t_n="<< x_t_n << endl;
+    cout << "近切点：y_t_n=" << y_t_n << endl;
+    cout << "远切点：x_t_f=" << x_t_f << endl;
+    cout << "远切点：y_t_f=" << y_t_f << endl;
+    //cout << "航向角：" << steering_angle << endl;
+    //cout << "圆心1横坐标=" << x_o_1 << endl;
+    //cout << "圆心1纵坐标=" << y_o_1 << endl;
+    //cout << "圆心2横坐标=" << x_o_2 << endl;
+    //cout << "圆心2纵坐标=" << y_o_2 << endl;
+    //cout << "平分线弧度=" << angle_3 << endl;
+    //cout << " min_rad=" << min_rad << endl;
+    //cout << " real_rad=" << real_rad << endl;
+    //   system("PAUSE");
+
+
+
+    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
+    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
+    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
+    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
+    dBocheAngle = steering_angle*180/PI;
+
+    double disA = GetDistance(aimGps,nowGps);
+
+    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
+        return 1;
+    }
+    return 0;
+
+}
+
+
+double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
+    double L_c = 4.749;//车长
+    double rad_1;
+    double rad_2;
+    double L_k = 1.931;//车宽
+    double L = 2.95;//轴距
+    double L_f =1.2 ;//前悬
+    double L_r =0.7 ;//后悬
+    double R_min =6.5 ;//最小转弯半径
+    *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
+    //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
+    //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
+    *min_rad = 45 * PI / 180;// rad_1 - rad_2;
+    return 0;
+}
+
+
+double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {
+
+    if (x_1 > 0 && y_1 > 0)
+    {
+        *real_rad = PI*0.5 - min_rad;
+        *x_2 = R_M - R_M*cos(min_rad);
+        *y_2 = R_M*sin(min_rad) + 0.5;
+    }
+    else
+    {
+        *real_rad = PI*0.5 + min_rad;
+        *x_2 = R_M*cos(min_rad) - R_M;
+        *y_2 = R_M*sin(min_rad) + 0.5;
+    }
+    return 0;
+
+}
+
+
+double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
+    double b1, b2;
+    double k1, k2;
+    if (ange1!=(PI*0.5))
+    {
+        k1 = tan(ange1);
+        b1 = y_1 - k1*x_1;
+        k2 = tan(real_rad);
+        b2 = y_2 - k2*x_2;
+        *x_3 = (b2 - b1) / (k1 - k2);
+        *y_3 = k2*(*x_3) + b2;
+    }
+    else
+    {
+        k2 = tan(real_rad);
+        b2 = y_2 - k2*x_2;
+        *x_3 = x_1;
+        *y_3 = k2*(*x_3) + b2;
+    }
+    return 0;
+}
+
+
+double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
+    double k1, k2;
+    double  angle_j;
+    k2 = tan(real_rad);
+    if (ange1 != (PI*0.5))
+    {
+        k1 = tan(ange1);
+        angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角
+
+        if (x_1 > 0 && y_1 > 0)
+        {
+            *angle_3 = angle_j*0.5 - min_rad + PI;
+        }
+        else
+        {
+            *angle_3 = min_rad - angle_j*0.5;
+        }
+    }
+    else
+    {
+        angle_j = min_rad;//两直线夹角
+        if (x_1 > 0 && y_1 > 0)
+        {
+            *angle_3 = angle_j*0.5 - min_rad + PI;
+        }
+        else
+        {
+            *angle_3 = min_rad - angle_j*0.5;
+        }
+    }
+    return 0;
+}
+
+
+double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
+                                        double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
+    double b2, b3, k2, k3;
+    b2 = y_2 - tan(real_rad)*x_2;
+    b3 = y_3 - tan(angle_3)*x_3;
+    k2 = tan(real_rad);
+    k3 = tan(angle_3);
+    *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
+    *y_o_1 = k3*(*x_o_1) + b3;
+
+    *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
+    *y_o_2 = k3*(*x_o_2) + b3;
+    return 0;
+}
+
+
+double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
+                                                double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
+                                                double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
+{
+    double x_o, y_o;
+    double b2, b3, k1, k2, k3;
+    //double car_pos[3] = { x_1,y_1,k1 };
+    double parking_pos[2] = { x_2,y_2 };
+    //double t[4][2];
+    double p[4];
+    double  s1, s2; //切点与车起始位置的距离
+    double  min;
+    int  min_i;
+    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
+    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
+    double t[4][2];
+    k1 = tan(ange1);
+    b2 = y_2 - tan(real_rad)*x_2;
+    b3 = y_3 - tan(real_rad)*x_3;
+    k2 = tan(real_rad);//另一条直线斜率
+    k3 = tan(angle_3);//垂直平分线斜率
+    //圆心1和2切点*********************************************
+    if (x_1 > 0 && y_1 > 0)//第一象限
+    {
+        if (ange1 == (PI*0.5))
+        {
+            x_t_1 = x_1;
+            y_t_1 = y_o_1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            x_t_3 = x_1;
+            y_t_3 = y_o_2;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+
+        }
+        else
+        {
+            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+        }
+    }
+    else
+    {
+        if (ange1 == 0)
+        {
+            x_t_1 = 0 - x_1;
+            y_t_1 = y_o_1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            x_t_3 = 0 - x_1;
+            y_t_3 = y_o_2;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+        }
+        else
+        {
+            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+        }
+
+    }
+
+    //圆心1和2切点*********************************************
+
+    t[0][0] = x_t_1;
+    t[0][1] = y_t_1;
+    t[1][0] = x_t_2;
+    t[1][1] = y_t_2;
+    t[2][0] = x_t_3;
+    t[2][1] = y_t_3;
+    t[3][0] = x_t_4;
+    t[3][1] = y_t_4;
+    for (int i = 0; i < 4; i++)
+    {
+
+        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
+
+    }
+    min = p[0];
+    min_i = 0;
+    for (int i = 1; i < 4; i++)
+    {
+
+        if (p[i] < min)
+        {
+            min = p[i]; min_i = i;
+        }
+    }
+    if (min_i < 2)
+    {
+        x_o = x_o_1;
+        y_o = y_o_1;
+        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
+        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
+        if (s1 < s2)
+        {
+            *x_t_n = x_t_1;
+            *y_t_n = y_t_1;
+            *x_t_f = x_t_2;
+            *y_t_f = y_t_2;
+        }
+        else
+        {
+            *x_t_n = x_t_2;
+            *y_t_n = y_t_2;
+            *x_t_f = x_t_1;
+            *y_t_f = y_t_1;
+
+        }
+    }
+    else
+    {
+        x_o = x_o_2;
+        y_o = y_o_2;
+        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
+        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
+
+        if (s1 < s2)
+        {
+
+            *x_t_n = x_t_3;
+            *y_t_n = y_t_3;
+            *x_t_f = x_t_4;
+            *y_t_f = y_t_4;
+        }
+        else
+        {
+            *x_t_n = x_t_4;
+            *y_t_n = y_t_4;
+            *x_t_f = x_t_3;
+            *y_t_f = y_t_3;
+        }
+
+
+
+    }
+
+    return 0;
+
+}
+
+
+int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
+{
+    int index = -1;
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+    float minDis=20;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis=tmpdis;
+        }
+    }
+    return index;
+}
+
+
+double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps) {
+    double obsSpeed = 0 - realSecSpeed;
+    double minDis = iv::MaxValue;
+    FrenetPoint esr_obs_F_point;
+    for (int i = 0; i < 64; i++)
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
+        {
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
+
+            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+            {
+                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+                if (tmpDis < minDis)
+                {
+                    minDis = tmpDis;
+//                    esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
+                    esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
+//                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
+                    double speedx=ServiceCarStatus.obs_radar[i].speed_x;  //障碍物相对于车辆x轴的速度
+                    double speedy=ServiceCarStatus.obs_radar[i].speed_y;  //障碍物相对于车辆y轴的速度
+                    double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
+                    //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
+                    //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
+                    double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);
+
+                    obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
+                }
+            }
+        }
+
+    return obsSpeed;
+}
+
+
+int iv::decition::Compute00::getEsrIndexByFrenet(std::vector<Point2D> gpsTrace, FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps){
+    double minDistance = numeric_limits<double>::max();
+    int minDis_index=-1;
+
+    for(int i=0; i<64; ++i){
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
+            //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+            //将毫米波障碍物位置转换到frenet坐标系下
+//            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
+            esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
+
+            //如果障碍物与道路的横向距离d<=3.0*Veh_Width / 4.0，则认为道路上有障碍物。
+            //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
+            //minDistance、minDis_index用来统计最近的障碍物信息。
+            if(abs(esrObsPoint.d)<=(3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs)){
+                if(esrObsPoint.s<minDistance){
+                    minDistance = esrObsPoint.s;
+                    minDis_index = i;
+                }
+            }
+        }
+    }
+    return minDis_index;
+}
+
+
+
+
+std::vector<std::vector<iv::GPSData>> gmapsL;
+std::vector<std::vector<iv::GPSData>> gmapsR;

src/decition/decition_brain_sf/decition/adc_tools/compute_00.h

@@ -1,101 +1,101 @@
-#pragma once
-#ifndef _IV_DECITION_COMPUTE_00_
-#define _IV_DECITION_COMPUTE_00_
-
-#include <common_sf/gps_type.h>
-#include <common_sf/obstacle_type.h>
-#include <decition/decition_type.h>
-#include<vector> 
-#include <decition/decide_gps_00.h>
-
-namespace iv {
-    namespace decition {
-    //根据传感器传输来的信息作出决策
-        class Compute00 {
-        public:
-            Compute00();
-            ~Compute00();
-
-            static   double maxAngle;
-            static	 double angleLimit;  //角度限制
-            static	 double lastEA;      //上一次角度误差
-            static	 double lastEP;      //上一次位置误差
-            static	 double decision_Angle;  //决策角度
-            static	 double lastAng;         //上次角度
-            static   int lastEsrID;          //上次毫米波障碍物ID
-            static   int  lastEsrCount;      //毫米波障碍物累计次数
-
-            static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
-            static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid
-
-            static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
-            static double bocheAngle,dBocheAngle;
-
-            static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
-            static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
-
-            static int getDesiredSpeed(std::vector<GPSData> gpsMap);
-
-            static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
-            static double getAveDef(std::vector<Point2D> farTrace);
-            static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
-            static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
-            static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
-
-
-            static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
-            static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint);
-            static int getRearEsrIndex(std::vector<Point2D> gpsTrace , int roadNum);
-            static int getEsrIndexAvoid(std::vector<Point2D> gpsTrace);
-
-            static double getObsSpeed(Point2D obsPoint, double realSecSpeed);
-
-            static double getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX);
-            static double getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX);
-
-            static std::vector<GPSData>  getBesideGpsMapLine(iv::GPS_INS now_gps_ins, std::vector<iv::GPSData>gpsMapLine, float avoidX);
-
-            static double getDecideAngleByLane(double realSpeed);
-
-            static double getDecideAngleByLanePID(double realSpeed);
-
-            static double bocheCompute(GPS_INS nowGps, GPS_INS aimGps);
-
-            static	double pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1);
-
-            static double getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps);
-
-            static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);
-
-            static double min_rad_zhuanxiang(double *R_M, double *min_rad);
-            static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
-                                    double *x_2, double *y_2, double *real_rad);
-            static double liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,
-                                                double ange1,double real_rad,double *x_3, double *y_3);
-            static double chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,
-                                                   double min_rad,double *angle_3);
-            static double yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,
-                                  double angle_3,double R_M,double *x_o_1, double *y_o_1, double *x_o_2, double *y_0_2);
-            static double yuanxin_qiedian(double angel,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
-                                          double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,
-                                          double real_rad,double angle_3,double R_M,
-                                          double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f);
-
-            static int getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap);
-
-            static int getEsrIndexByFrenet(std::vector<Point2D> gpsTrace,FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps);
-            double getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const std::vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps);
-
-        private:
-
-        };
-    }
-}
-extern std::vector<std::vector<iv::GPSData>> gmapsL;
-extern std::vector<std::vector<iv::GPSData>> gmapsR;
-
-extern std::vector<int> lastEsrIdVector;
-extern std::vector<int> lastEsrCountVector;
-
-#endif // !_IV_DECITION_COMPUTE_00_
-
+#pragma once
+#ifndef _IV_DECITION_COMPUTE_00_
+#define _IV_DECITION_COMPUTE_00_
+
+#include <common_sf/gps_type.h>
+#include <common_sf/obstacle_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+#include <decition/decide_gps_00.h>
+
+namespace iv {
+    namespace decition {
+    //根据传感器传输来的信息作出决策
+        class Compute00 {
+        public:
+            Compute00();
+            ~Compute00();
+
+            static   double maxAngle;
+            static	 double angleLimit;  //角度限制
+            static	 double lastEA;      //上一次角度误差
+            static	 double lastEP;      //上一次位置误差
+            static	 double decision_Angle;  //决策角度
+            static	 double lastAng;         //上次角度
+            static   int lastEsrID;          //上次毫米波障碍物ID
+            static   int  lastEsrCount;      //毫米波障碍物累计次数
+
+            static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
+            static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid
+
+            static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
+            static double bocheAngle,dBocheAngle;
+
+            static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
+            static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
+
+            static int getDesiredSpeed(std::vector<GPSData> gpsMap);
+
+            static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
+            static double getAveDef(std::vector<Point2D> farTrace);
+            static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
+            static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+
+
+            static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint);
+            static int getRearEsrIndex(std::vector<Point2D> gpsTrace , int roadNum);
+            static int getEsrIndexAvoid(std::vector<Point2D> gpsTrace);
+
+            static double getObsSpeed(Point2D obsPoint, double realSecSpeed);
+
+            static double getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX);
+            static double getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX);
+
+            static std::vector<GPSData>  getBesideGpsMapLine(iv::GPS_INS now_gps_ins, std::vector<iv::GPSData>gpsMapLine, float avoidX);
+
+            static double getDecideAngleByLane(double realSpeed);
+
+            static double getDecideAngleByLanePID(double realSpeed);
+
+            static double bocheCompute(GPS_INS nowGps, GPS_INS aimGps);
+
+            static	double pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1);
+
+            static double getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps);
+
+            static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);
+
+            static double min_rad_zhuanxiang(double *R_M, double *min_rad);
+            static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
+                                    double *x_2, double *y_2, double *real_rad);
+            static double liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,
+                                                double ange1,double real_rad,double *x_3, double *y_3);
+            static double chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,
+                                                   double min_rad,double *angle_3);
+            static double yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,
+                                  double angle_3,double R_M,double *x_o_1, double *y_o_1, double *x_o_2, double *y_0_2);
+            static double yuanxin_qiedian(double angel,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
+                                          double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,
+                                          double real_rad,double angle_3,double R_M,
+                                          double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f);
+
+            static int getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap);
+
+            static int getEsrIndexByFrenet(std::vector<Point2D> gpsTrace,FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps);
+            double getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const std::vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps);
+
+        private:
+
+        };
+    }
+}
+extern std::vector<std::vector<iv::GPSData>> gmapsL;
+extern std::vector<std::vector<iv::GPSData>> gmapsR;
+
+extern std::vector<int> lastEsrIdVector;
+extern std::vector<int> lastEsrCountVector;
+
+#endif // !_IV_DECITION_COMPUTE_00_
+

src/decition/decition_brain_sf/decition/adc_tools/gnss_coordinate_convert.cpp

@@ -1,154 +1,154 @@
-#include <decition/adc_tools/gnss_coordinate_convert.h>
-
-void gps2xy(double J4, double K4, double *x, double *y)
-{
-    int L4 = (int)((K4 - 1.5) / 3) + 1;
-    double M4 = K4 - (L4 * 3);
-    double N4 = sin(J4 * 3.1415926536 / 180);
-    double O4 = cos(J4 * 3.1415926536 / 180);
-    double P4 = tan(J4 * 3.1415926536 / 180);
-    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
-    double R4 = sqrt(0.006738525414683) * O4;
-    double S4 = sqrt(1 + R4 * R4);
-    double T4 = 6399698.901783 / S4;
-    double U4 = (T4 / S4) / S4;
-    double V4 = O4 * M4 * 3.1415926536 / 180;
-    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
-    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
-    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
-    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
-
-    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
-    *x = 500000 + T4 * V4 * (Y4 + Z4);
-}
-
-
-
-//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
-void GaussProjCal(double longitude, double latitude, double *X, double *Y)
-{
-    int ProjNo = 0; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-    double a, f, e2, ee, NN, T, C, A, M, iPI;
-    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-    ZoneWide = 6; ////6度带宽
-    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-    ProjNo = (int)(longitude / ZoneWide);
-    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
-    longitude0 = longitude0 * iPI;
-    latitude0 = 0;
-    longitude1 = longitude * iPI; //经度转换为弧度
-    latitude1 = latitude * iPI; //纬度转换为弧度
-    e2 = 2 * f - f * f;
-    ee = e2 * (1.0 - e2);
-    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
-    T = tan(latitude1)*tan(latitude1);
-    C = ee * cos(latitude1)*cos(latitude1);
-    A = (longitude1 - longitude0)*cos(latitude1);
-    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
-        *e2 / 1024)*sin(2 * latitude1)
-        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
-    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
-    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
-        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
-    X0 = 1000000L * (ProjNo + 1) + 500000L;
-    Y0 = 0;
-    xval = xval + X0; yval = yval + Y0;
-    *X = xval;
-    *Y = yval;
-}
-
-
-/*
-//=======================zhaobo0904
-#define PI  3.14159265358979
-void GaussProjCal(double lon, double lat, double *X, double *Y)
-{
-    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
-    double a = 6378140.0;
-    double e2 = 0.006694384999588;
-    double ep2 = e2/(1-e2);
-
-    // 原点所在经度
-    double lon_origin = 6.0*int(lon/6) + 3.0;
-    lon_origin *= PI / 180.0;
-
-    double k0 = 0.9996;
-
-    // 角度转弧度
-    double lat1 = lat * PI / 180.0;
-    double lon1 = lon * PI / 180.0;
-
-
-    // 经线在该点处的曲率半径,
-    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
-
-
-    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
-    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
-    // 首先计算前四项的系数 a1~a4.
-    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
-    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
-    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
-    double a4 = (35*e2*e2*e2)/3072;
-    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
-
-    // 辅助量
-    double T = tan(lat1)*tan(lat1);
-    double C = ep2*cos(lat1)*cos(lat1);
-    double A = (lon1 - lon_origin)*cos(lat1);
-
-    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
-    *Y = M + N * tan(lat1) * ((A*A)/2 +
-                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
-                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
-
-
-    *Y *= k0;
-    return;
-}
-//==========================================================
-
-*/
-
-
-//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
-void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
-{
-    int ProjNo; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
-    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-    ZoneWide = 6; ////6度带宽
-    ProjNo = (int)(X / 1000000L); //查找带号
-    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
-    longitude0 = longitude0 * iPI; //中央经线
-    X0 = ProjNo * 1000000L + 500000L;
-    Y0 = 0;
-    xval = X - X0; yval = Y - Y0; //带内大地坐标
-    e2 = 2 * f - f * f;
-    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
-    ee = e2 / (1 - e2);
-    M = yval;
-    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
-    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
-                4 * u)
-            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
-    C = ee * cos(fai)*cos(fai);
-    T = tan(fai)*tan(fai);
-    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
-    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
-                                                                                       (fai)*sin(fai)));
-    D = xval / NN;
-    //计算经度(Longitude) 纬度(Latitude)
-    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
-                               *D*D*D*D / 120) / cos(fai);
-    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
-                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
-    //转换为度 DD
-    *longitude = longitude1 / iPI;
-    *latitude = latitude1 / iPI;
-}
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+/*
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+
+*/
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void GaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/decition/decition_brain_sf/decition/adc_tools/gnss_coordinate_convert.h

@@ -1,26 +1,26 @@
-#pragma once
-#ifndef _IV_PERCEPTION_GNSS_CONVERT_
-#define _IV_PERCEPTION_GNSS_CONVERT_
-
-#include <math.h>
-//double nmeaConvert2Lat(string lat)
-//{
-//	Console.WriteLine(lat);
-//	double nmea_d = double.Parse(lat.Substring(0, 2));
-//	double nmea_m = double.Parse(lat.Substring(2, 6));
-//	return nmea_d + nmea_m / 60;
-//}
-//
-//double nmeaConvert2Lon(string lon)
-//{
-//	Console.WriteLine(lon);
-//	double nmea_d = double.Parse(lon.Substring(0, 3));
-//	double nmea_m = double.Parse(lon.Substring(3, 7));
-//	return nmea_d + nmea_m / 60;
-//}
-
-void gps2xy(double , double , double *, double *);
-void GaussProjCal(double longitude, double latitude, double *X, double *Y);
-void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
-
-#endif // !_IV_PERCEPTION_GNSS_CONVERT_
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/decition/decition_brain_sf/decition/adc_tools/gps_distance.cpp

@@ -1,45 +1,45 @@
-#include <decition/adc_tools/gps_distance.h>
-#include <math.h>
-#define M_PI (3.1415926535897932384626433832795)
-
-// 计算弧度
-double iv::decition::rad(double d)
-{
-    const double PI = 3.1415926535898;
-    return d * PI / 180.0;
-}
-
-// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
-double iv::decition::CalcDistance(double fLati1, double fLong1, double fLati2, double fLong2)
-{
-    const float EARTH_RADIUS = 6378.137;
-    double radLat1 = rad(fLati1);
-    double radLat2 = rad(fLati2);
-    double a = radLat1 - radLat2;
-
-    double b = rad(fLong1) - rad(fLong2);
-    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));
-    s = s * EARTH_RADIUS;
-    //	s = (int)(s * 10000000) / 10000;
-    return s;
-}
-
-
-// 从直角坐标两点的直线距离，单位是米
-double iv::decition::DirectDistance(double x1, double y1, double x2, double y2)
-{
-    double s = sqrt(pow((x1-x2), 2) +pow((y1-y2), 2));
-    return s;
-}
-
-
-// 从直角坐标计算两点的夹角
-double iv::decition::DirectAngle(double x1, double y1, double x2, double y2)
-{
-    double angle = atan((y2 - y1) / (x2 - x1)) * 180 / M_PI;
-    return angle;
-}
-
-
-
-
+#include <decition/adc_tools/gps_distance.h>
+#include <math.h>
+#define M_PI (3.1415926535897932384626433832795)
+
+// 计算弧度
+double iv::decition::rad(double d)
+{
+    const double PI = 3.1415926535898;
+    return d * PI / 180.0;
+}
+
+// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
+double iv::decition::CalcDistance(double fLati1, double fLong1, double fLati2, double fLong2)
+{
+    const float EARTH_RADIUS = 6378.137;
+    double radLat1 = rad(fLati1);
+    double radLat2 = rad(fLati2);
+    double a = radLat1 - radLat2;
+
+    double b = rad(fLong1) - rad(fLong2);
+    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));
+    s = s * EARTH_RADIUS;
+    //	s = (int)(s * 10000000) / 10000;
+    return s;
+}
+
+
+// 从直角坐标两点的直线距离，单位是米
+double iv::decition::DirectDistance(double x1, double y1, double x2, double y2)
+{
+    double s = sqrt(pow((x1-x2), 2) +pow((y1-y2), 2));
+    return s;
+}
+
+
+// 从直角坐标计算两点的夹角
+double iv::decition::DirectAngle(double x1, double y1, double x2, double y2)
+{
+    double angle = atan((y2 - y1) / (x2 - x1)) * 180 / M_PI;
+    return angle;
+}
+
+
+
+

src/decition/decition_brain_sf/decition/adc_tools/gps_distance.h

@@ -1,26 +1,26 @@
-#pragma once
-#ifndef _IV_DECITION_GPS_DISTANCE_
-#define _IV_DECITION_GPS_DISTANCE_
-
-namespace iv {
-	namespace decition {
-        // 计算弧度
-		double rad(double d);
-
-		// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
-		double CalcDistance(double , double , double , double );
-
-		//计算直角坐标距离
-		double DirectDistance(double, double, double, double);
-
-		//计算直角坐标角度
-		double DirectAngle(double, double, double, double);
-
-
-	}
-}
-
-
-
-
-#endif // !_IV_DECITION_GPS_DISTANCE_
+#pragma once
+#ifndef _IV_DECITION_GPS_DISTANCE_
+#define _IV_DECITION_GPS_DISTANCE_
+
+namespace iv {
+	namespace decition {
+        // 计算弧度
+		double rad(double d);
+
+		// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
+		double CalcDistance(double , double , double , double );
+
+		//计算直角坐标距离
+		double DirectDistance(double, double, double, double);
+
+		//计算直角坐标角度
+		double DirectAngle(double, double, double, double);
+
+
+	}
+}
+
+
+
+
+#endif // !_IV_DECITION_GPS_DISTANCE_

src/decition/decition_brain_sf/decition/adc_tools/obs_predict.cpp

@@ -1,138 +1,138 @@
-#include <decition/adc_tools/transfer.h>
-#include <decition/decition_type.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-using namespace std;
-#define PI (3.1415926535897932384626433832795)
-
-
-
-///大地转车体
-iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
-{
-    double x_vehicle, y_vehicle;
-    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
-
-    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
-    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
-
-    x_vehicle = -cos(angle) * distance;
-    y_vehicle = sin(angle) * distance;
-    return  iv::Point2D(x_vehicle, y_vehicle);
-}
-
-///车体转大地
-iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
-{
-    iv::GPS_INS gps_ins;
-    double x_vehicle, y_vehicle;
-    double theta = atan2(x_path, y_path);
-
-    double distance = sqrt(x_path * x_path + y_path * y_path);
-    double angle = (pos.ins_heading_angle / 180 * PI + theta);
-
-    x_vehicle = pos.gps_x + distance * sin(angle);
-    y_vehicle = pos.gps_y + distance * cos(angle);
-    gps_ins.gps_x = x_vehicle;
-    gps_ins.gps_y = y_vehicle;
-
-    return  gps_ins;
-}
-
-double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
-{
-    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
-}
-
-double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
-{
-    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
-}
-
-
-
-
-//GPS转大地坐标
-void iv::decition::BLH2XYZ(iv::GPS_INS gp)
-{
-    int nFlag = 2;
-
-    double B = gp.gps_lat;
-
-    double L = gp.gps_lng;
-
-
-
-    double N, E, h;
-    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度
-
-    double a = 6378245.0;            //地球半径  北京6378245
-    double F = 298.257223563;        //地球扁率
-    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换
-
-    double f = 1 / F;
-    double b = a * (1 - f);
-    double ee = (a * a - b * b) / (a * a);
-    double e2 = (a * a - b * b) / (b * b);
-    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
-    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
-    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
-    double gm = 15 * n2 / 16 - 15 * n4 / 32;
-    double dt = -35 * n3 / 48 + 105 * n5 / 256;
-    double ep = 315 * n4 / 512;
-
-    B = B * iPI;
-    L = L * iPI;
-    L0 = L0 * iPI;
-
-    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
-    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
-    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
-    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
-    double yt = e2 * cos(B) * cos(B);
-    N = lB;
-    N += t * Nn * cl2 / 2;
-    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
-    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
-    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
-
-    E = Nn * cl;
-    E += Nn * cl3 * (1 - t2 + yt) / 6;
-    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
-    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
-
-    E += 500000;
-    if (nFlag == 1)
-    {
-        //UTM投影
-        N = 0.9996 * N;
-        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
-    }
-    if (nFlag == 2)
-    {
-        //UTM投影
-        N = 0.9999 * N;
-        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
-    }
-
-    //原
-    //pt2d.x = N;
-    //pt2d.y = E;
-    //
-    gp.gps_x = E - 280000;
-    gp.gps_y = N - 4325000;
-
-}
-
-
-double iv::decition::GetL0InDegree(double dLIn)
-{
-    //3°带求带号及中央子午线经度(d的形式)
-    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
-    double L = dLIn;//d.d
-    double L_ddd_Style = L;
-    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
-    double L0 = ZoneNumber * 3.0;//degree
-    return L0;
-}
+#include <decition/adc_tools/transfer.h>
+#include <decition/decition_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+using namespace std;
+#define PI (3.1415926535897932384626433832795)
+
+
+
+///大地转车体
+iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    double x_vehicle, y_vehicle;
+    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
+
+    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
+    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
+
+    x_vehicle = -cos(angle) * distance;
+    y_vehicle = sin(angle) * distance;
+    return  iv::Point2D(x_vehicle, y_vehicle);
+}
+
+///车体转大地
+iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    iv::GPS_INS gps_ins;
+    double x_vehicle, y_vehicle;
+    double theta = atan2(x_path, y_path);
+
+    double distance = sqrt(x_path * x_path + y_path * y_path);
+    double angle = (pos.ins_heading_angle / 180 * PI + theta);
+
+    x_vehicle = pos.gps_x + distance * sin(angle);
+    y_vehicle = pos.gps_y + distance * cos(angle);
+    gps_ins.gps_x = x_vehicle;
+    gps_ins.gps_y = y_vehicle;
+
+    return  gps_ins;
+}
+
+double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
+{
+    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
+}
+
+double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
+{
+    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
+}
+
+
+
+
+//GPS转大地坐标
+void iv::decition::BLH2XYZ(iv::GPS_INS gp)
+{
+    int nFlag = 2;
+
+    double B = gp.gps_lat;
+
+    double L = gp.gps_lng;
+
+
+
+    double N, E, h;
+    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度
+
+    double a = 6378245.0;            //地球半径  北京6378245
+    double F = 298.257223563;        //地球扁率
+    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换
+
+    double f = 1 / F;
+    double b = a * (1 - f);
+    double ee = (a * a - b * b) / (a * a);
+    double e2 = (a * a - b * b) / (b * b);
+    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
+    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
+    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
+    double gm = 15 * n2 / 16 - 15 * n4 / 32;
+    double dt = -35 * n3 / 48 + 105 * n5 / 256;
+    double ep = 315 * n4 / 512;
+
+    B = B * iPI;
+    L = L * iPI;
+    L0 = L0 * iPI;
+
+    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
+    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
+    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
+    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
+    double yt = e2 * cos(B) * cos(B);
+    N = lB;
+    N += t * Nn * cl2 / 2;
+    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
+    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
+    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
+
+    E = Nn * cl;
+    E += Nn * cl3 * (1 - t2 + yt) / 6;
+    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
+    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
+
+    E += 500000;
+    if (nFlag == 1)
+    {
+        //UTM投影
+        N = 0.9996 * N;
+        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
+    }
+    if (nFlag == 2)
+    {
+        //UTM投影
+        N = 0.9999 * N;
+        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
+    }
+
+    //原
+    //pt2d.x = N;
+    //pt2d.y = E;
+    //
+    gp.gps_x = E - 280000;
+    gp.gps_y = N - 4325000;
+
+}
+
+
+double iv::decition::GetL0InDegree(double dLIn)
+{
+    //3°带求带号及中央子午线经度(d的形式)
+    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
+    double L = dLIn;//d.d
+    double L_ddd_Style = L;
+    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
+    double L0 = ZoneNumber * 3.0;//degree
+    return L0;
+}

src/decition/decition_brain_sf/decition/adc_tools/transfer.h

@@ -1,27 +1,27 @@
-#pragma once
-#ifndef _IV_DECITION_TRANSFER_
-#define _IV_DECITION_TRANSFER_
-
-#include <common_sf/gps_type.h>
-#include <decition/decition_type.h>
-#include<vector> 
-
-namespace iv {
-    namespace decition {
-        //根据传感器传输来的信息作出决策
-
-        double GetL0InDegree(double dLIn);
-
-        void BLH2XYZ(GPS_INS gps_ins);
-
-        ///大地转车体
-        Point2D Coordinate_Transfer(double x_path, double y_path, GPS_INS pos);
-
-        ///车体转大地
-        GPS_INS Coordinate_UnTransfer(double x_path, double y_path, GPS_INS pos);
-        double GetDistance(Point2D x1, Point2D x2);
-        double GetDistance(GPS_INS p1, GPS_INS p2);
-    }
-}
-
-#endif // ! _IV_DECITION_TRANSFER_
+#pragma once
+#ifndef _IV_DECITION_TRANSFER_
+#define _IV_DECITION_TRANSFER_
+
+#include <common_sf/gps_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+
+namespace iv {
+    namespace decition {
+        //根据传感器传输来的信息作出决策
+
+        double GetL0InDegree(double dLIn);
+
+        void BLH2XYZ(GPS_INS gps_ins);
+
+        ///大地转车体
+        Point2D Coordinate_Transfer(double x_path, double y_path, GPS_INS pos);
+
+        ///车体转大地
+        GPS_INS Coordinate_UnTransfer(double x_path, double y_path, GPS_INS pos);
+        double GetDistance(Point2D x1, Point2D x2);
+        double GetDistance(GPS_INS p1, GPS_INS p2);
+    }
+}
+
+#endif // ! _IV_DECITION_TRANSFER_

src/decition/decition_brain_sf/decition/brain.cpp

@@ -1,1250 +1,1250 @@
-#include <decition/brain.h>
-#include <fstream>
-#include <time.h>
-#include <exception>
-#include <decition/adc_tools/gnss_coordinate_convert.h>
-#include <common/logout.h>
-#include <decition/adc_tools/transfer.h>
-
-#include <iostream>
-#include "xmlparam.h"
-#include "qcoreapplication.h"
-
-
-extern std::string gstrmemdecition;
-extern std::string gstrmembrainstate;
-extern iv::Ivlog * givlog;
-extern std::string gstrmemchassis;
-
-#define LOG_ENABLE
-
-namespace iv {
-namespace decition {
-iv::decition::BrainDecition * gbrain;
-
-
-        void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            gbrain->UpdateMap(strdata,nSize);
-            gbrain->UpdateSate();
-        }
-
-        void ListenVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            gbrain->UpdateVbox(strdata,nSize);
-            gbrain->UpdateSate();
-        }
-
-        void ListenHMI(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            gbrain->UpdateHMI(strdata,nSize);
-        }
-
-        void ListenPlatform(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            gbrain->UpdatePlatform(strdata,nSize);
-        }
-
-        void ListenGroup(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            gbrain->UpdateGroupInfo(strdata,nSize);
-        }
-
-    /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            iv::formation_map_index::map map;
-            if(false == map.ParseFromArray(strdata,nSize))
-            {
-                givlog->error("GPS","[%s] Listencansend Parse fail.",__func__);
-                return;
-            }else{
-               gbrain->UpdateSate();
-            }
-        //    map.index()-1  map index num.
-        }*/
-
-    }
-
-}
-
-
-
-iv::decition::BrainDecition::BrainDecition()
-{
-    //    mpasd = new Adcivstatedb();
-    look1 = 0.0;
-    look2 = 0.0;
-    look3 = 0.0;
-    look4 = 0.0;
-    look5 = 0.0;
-    look6 = 0.0;
-    look7 = 0.0;
-    obs_lidar_grid_ = NULL;
-    old_obs_lidar_grid_ = NULL;
-    obs_camera_ = NULL;
-    obs_fusion_grid_ = NULL;
-    //    controller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());
-
-    //    carcall = new iv::carcalling::carcalling();
-    eyes = new iv::perception::Eyes();
-    //	decitionMaker = new iv::decition::DecitionMaker();
-
-    decitionGps00 = new iv::decition::DecideGps00();
-    decitionLine00=new iv::decition::DecideLine00();
-
-    //	decitionGpsUnit = new iv::decition::DecideGpsUnit();
-
-    //	decitionExecuter = new iv::decition::DecitionExecuter(controller);
-
-    iv::decition::gbrain = this;
-
-
-    mpa = iv::modulecomm::RegisterRecv("tracemap",iv::decition::ListenTraceMap);
-
-    mpvbox = iv::modulecomm::RegisterRecv("vbox",iv::decition::ListenVbox);
-
-    mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
-
-
-    mpaToPlatform = iv::modulecomm::RegisterSend("toplatform",10*sizeof(iv::platform::PlatFormMsg),10);
-    mpaDecition = iv::modulecomm::RegisterSend(gstrmemdecition.data(),10*sizeof(iv::decition::DecitionBasic),1);
-
-    mpaHMI = iv::modulecomm::RegisterRecv("hmi",iv::decition::ListenHMI);
-
-    mpaPlatform = iv::modulecomm::RegisterRecv("platformfrom",iv::decition::ListenPlatform);
-
-    mpaGroup = iv::modulecomm::RegisterRecv("p900_send",iv::decition::ListenGroup);
- //   iv::modulecomm::RegisterRecv("map_index",ListenMap_change_req);
-
-
-
-
-    mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
-
-    ModuleFun funchassis =std::bind(&iv::decition::BrainDecition::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    void * pa = iv::modulecomm::RegisterRecvPlus(gstrmemchassis.data(),funchassis);
-
-    mpaPlanTrace = iv::modulecomm::RegisterSend("plantrace",100000,1);
-
-    mTime.start();
-    mnOldTime = mTime.elapsed();
-
-}
-
-iv::decition::BrainDecition::~BrainDecition()
-{
-}
-
-
-void iv::decition::BrainDecition::inialize() {
-    //	controller->inialize();
-    eyes->inialize();
-    //    carcall->start();
-}
-
-
-void iv::decition::BrainDecition::run() {
-
-    double last = 0;
-
-    iv::decition::Decition decition_gps = NULL;
-    iv::decition::Decition decition_lidar = NULL;
-    iv::decition::Decition decition_radar = NULL;
-    iv::decition::Decition decition_camera = NULL;
-    iv::decition::Decition decition_localmap = NULL;
-
-    iv::decition::Decition dgtemp(new iv::decition::DecitionBasic);
-    decition_gps = dgtemp;
-    decition_gps->brake = 0;
-    decition_gps->accelerator = 0;
-    decition_gps->wheel_angle = 0;
-
-    //controller->auto_drive_mode_enable(true);
-
-    /*Decition test(new DecitionBasic);
-    while (true)
-    {
-        test->accelerator = 1.2;
-        test->wheel_angle = 0;
-        decitionExecuter->executeDecition(test);
-        Sleep(5000);
-
-        test->accelerator = 0;
-        test->wheel_angle = 0;
-        decitionExecuter->executeDecition(test);
-        Sleep(5000);
-
-        test->accelerator = -1.2;
-        test->wheel_angle = 0;
-        decitionExecuter->executeDecition(test);
-        Sleep(5000);
-
-        test->accelerator = 0;
-        test->wheel_angle = 45;
-        decitionExecuter->executeDecition(test);
-        Sleep(5000);
-
-        test->accelerator = 0;
-        test->wheel_angle = 0;
-        decitionExecuter->executeDecition(test);
-        Sleep(5000);
-
-        test->accelerator = 0;
-        test->wheel_angle = -60;
-        decitionExecuter->executeDecition(test);
-        Sleep(5000);
-
-    }*/
-
-    //   std::cout<<"start run."<<std::endl;
-
-    //   ServiceCarStatus.mbRunPause = false;
-
-    bool bRun;
-    int nValueSize;
-
-    //    if(mpasd->LoadState("runstate",(char *)&bRun,sizeof(bool),&nValueSize) == true)
-    //    {
-    //        if(bRun)
-    //        {
-    //            ServiceCarStatus.mbRunPause = false;
-    //        }
-    //        else
-    //        {
-    //            ServiceCarStatus.mbRunPause = true;
-    //        }
-    //    }
-
-    char * strmap = new char[10000000];
-    int nMapSize;
-
-    //    if(mpasd->LoadState("map",strmap,10000000,&nMapSize) == true)
-    //    {
-    //        UpdateMap(strmap,nMapSize);
-    //    }
-
-    QString strpath = QCoreApplication::applicationDirPath();
-    strpath = strpath + "/ADS_decision.xml";
-    iv::xmlparam::Xmlparam xp(strpath.toStdString());
-    ServiceCarStatus.msysparam.mstrvin = xp.GetParam("vin","10000000000000001");
-    ServiceCarStatus.msysparam.mstrid = xp.GetParam("id","1");
-    ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
-    ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");
-
-
-
-    /*=============     20200113 apollo_fu  add ===========*/
-    std::string str_zhuchetime = xp.GetParam("zhuchetime","16");
-    ServiceCarStatus.msysparam.mzhuchetime =atoi(str_zhuchetime.data());
-    /*============= end ================================== */
-
-    std::string strepsoff = xp.GetParam("epsoff","0.0");
-    ServiceCarStatus.mfEPSOff = atof(strepsoff.data());
-
-    std::string strroadmode_vel = xp.GetParam("roadmode0","10");
-    ServiceCarStatus.mroadmode_vel.mfmode0 = atof(strroadmode_vel.data());
-
-    strroadmode_vel = xp.GetParam("roadmode11","30");
-    ServiceCarStatus.mroadmode_vel.mfmode11 = atof(strroadmode_vel.data());
-
-
-    strroadmode_vel = xp.GetParam("roadmode14","15");
-    ServiceCarStatus.mroadmode_vel.mfmode14 = atof(strroadmode_vel.data());
-
-    //float a = ServiceCarStatus.mroadmode_vel.mfmode14;
-    //cout<<"........"<<a<<endl;
-
-    strroadmode_vel = xp.GetParam("roadmode15","15");
-    ServiceCarStatus.mroadmode_vel.mfmode15 = atof(strroadmode_vel.data());
-
-    /*==================== 20200113    apollo_fu add =================*/
-    strroadmode_vel = xp.GetParam("roadmode5","10");
-    ServiceCarStatus.mroadmode_vel.mfmode5 = atof(strroadmode_vel.data());
-
-    strroadmode_vel = xp.GetParam("roadmode13","5");
-    ServiceCarStatus.mroadmode_vel.mfmode13 = atof(strroadmode_vel.data());
-
-    strroadmode_vel = xp.GetParam("roadmode16","8");
-    ServiceCarStatus.mroadmode_vel.mfmode16 = atof(strroadmode_vel.data());
-
-    strroadmode_vel = xp.GetParam("roadmode17","8");
-    ServiceCarStatus.mroadmode_vel.mfmode17 = atof(strroadmode_vel.data());
-
-    strroadmode_vel = xp.GetParam("roadmode18","5");
-    ServiceCarStatus.mroadmode_vel.mfmode18 = atof(strroadmode_vel.data());
-
-
-
-    /*=========================    end   =============================*/
-
-    std::string group_cotrol = xp.GetParam("group","false");
-    if(group_cotrol=="true"){
-        ServiceCarStatus.group_control=true;
-    }else{
-        ServiceCarStatus.group_control=false;
-    }
-
-    std::string speed_control = xp.GetParam("speed","false");
-    if(speed_control=="true"){
-        ServiceCarStatus.speed_control=true;
-    }else{
-        ServiceCarStatus.speed_control=false;
-    }
-
-
-
-    std::string strparklat = xp.GetParam("parklat","39.0624557");
-    std::string strparklng = xp.GetParam("parklng","117.3575493");
-    std::string strparkheading = xp.GetParam("parkheading","112.5");
-    std::string strparktype = xp.GetParam("parktype","1");
-
-    ServiceCarStatus.mfParkLat = atof(strparklat.data());
-    ServiceCarStatus.mfParkLng = atof(strparklng.data());
-    ServiceCarStatus.mfParkHeading = atof(strparkheading.data());
-    ServiceCarStatus.mnParkType = atoi(strparktype.data());
-
-    ServiceCarStatus.mLidarRotation = atof(xp.GetParam("LidarRotation","90.0").data());
-    ServiceCarStatus.mLidarRangeUnit = atof(xp.GetParam("LidarRangeUnit","5.0").data());
-
-
-
-    std::string lightlon = xp.GetParam("lightlon","0");
-    std::string lightlat = xp.GetParam("lightlat","0");
-    ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data());
-    ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());
-
-
-
-    //mobileEye
-    std::string   timeWidth =xp.GetParam("waitGpsTimeWidth","5000");
-    std::string   garageLong =xp.GetParam("outGarageLong","10");
-    ServiceCarStatus.waitGpsTimeWidth=atof(timeWidth.data());
-    ServiceCarStatus.outGarageLong=atof(garageLong.data());
-    //mobileEye end
-
-
-    //lidar start
-    std::string lidarGpsXiuzheng = xp.GetParam("lidarGpsXiuzheng","1.0");
-    std::string radarGpsXiuzheng = xp.GetParam("radarGpsXiuzheng","3.0");
-    std::string rearRadarGpsXiuzheng = xp.GetParam("rearRadarGpsXiuzheng","0.5");
-    std::string rearLidarGpsXiuzheng = xp.GetParam("rearLidarGpsXiuzheng","1.0");
-    std::string frontGpsXiuzheng = xp.GetParam("frontGpsXiuzheng","3.0");
-    std::string rearGpsXiuzheng = xp.GetParam("rearGpsXiuzheng","1.0");
-    std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
-    std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
-    std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
-    if(strexternmpc == "true")
-    {
-        mbUseExternMPC = true;
-    }
-
-    ServiceCarStatus.msysparam.lidarGpsXiuzheng = atof(lidarGpsXiuzheng.data());
-    ServiceCarStatus.msysparam.radarGpsXiuzheng = atof(radarGpsXiuzheng.data());
-    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng = atof(rearRadarGpsXiuzheng.data());
-    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng = atof(rearLidarGpsXiuzheng.data());
-    ServiceCarStatus.msysparam.frontGpsXiuzheng = atof(frontGpsXiuzheng.data());
-    ServiceCarStatus.msysparam.rearGpsXiuzheng = atof(rearGpsXiuzheng.data());
-    ServiceCarStatus.msysparam.gpsOffset_x = atof(gpsOffset_x.data());
-    ServiceCarStatus.msysparam.gpsOffset_y = atof(gpsOffset_y.data());
-    // lidar end
-
-    std::string strObsPredict = xp.GetParam("obsPredict","false");  //If Use MPC set true
-    if(strObsPredict == "true")
-    {
-        ServiceCarStatus.useObsPredict = true;
-    }
-    //map
-
-    mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
-
-
-    while (eyes->isAllSensorRunning())
-    {
-        if(navigation_data.size() <1)
-        {
-            iv::map::mapreq x;
-            x.set_maptype(1);
-            int nsize = x.ByteSize();
-            char * str = new char[nsize];
-            if(x.SerializeToArray(str,nsize))
-            {
-                iv::modulecomm::ModuleSendMsg(mpamapreq,str,nsize);
-            }
-            else
-            {
-                std::cout<<"iv::map::mapreq serialize error."<<std::endl;
-            }
-            delete str;
-        }
-
-
-        iv::GPSData gps_data_;			//gps 惯导数据
-
-        //       qDebug("size = %d",navigation_data.size());
-
-
-
-        std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
-
-        bool brun =ServiceCarStatus.mbRunPause;
-        if(ServiceCarStatus.mnBocheComplete > 0)
-        {
-            ServiceCarStatus.mbBrainCtring = false;
-            ServiceCarStatus.mbRunPause = true;
-            ServiceCarStatus.mnBocheComplete--;
-        }
-        if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
-        {
-            ServiceCarStatus.mbBrainCtring = false;
-            decition_gps->brake = 6;
-            decition_gps->accelerator = -0.5;
-            decition_gps->wheel_angle = 0;
-            decition_gps->torque = 0;
-            decition_gps->mode = 0;
-            //decitionExecuter->executeDecition(decition_gps);
-                   ShareDecition(decition_gps);  //apollo_fu 20200413 qudiaozhushi
-            ServiceCarStatus.mfAcc = decition_gps->accelerator;
-            ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
-            ServiceCarStatus.mfBrake = decition_gps->brake;
-
-            iv::brain::brainstate xbs;
-            xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
-            xbs.set_mbbrainrunning(false);
-            xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
-            xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
-            xbs.set_mfobs(ServiceCarStatus.mObs);
-            xbs.set_decision_period(decision_period);
-            ShareBrainstate(xbs);    //apollo_fu 20200413 qudiaozhushi
-            //            iv::decition::VehicleStateBasic vsb;
-            //            vsb.mbBocheEnable = ServiceCarStatus.bocheEnable;
-            //            vsb.mfLidarObs = ServiceCarStatus.mLidarObs;
-            //            vsb.mfRadarObs = ServiceCarStatus.mRadarObs;
-            //            vsb.mfObs = ServiceCarStatus.mRadarObs;
-            //            vsb.decision_period = decision_period;
-            //            vsb.mbBrainRunning = false;
-            ////             mpaVechicleState->writemsg((char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
-            //            iv::modulecomm::ModuleSendMsg(mpaVechicleState,(char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
-
-#ifdef Q_OS_LINUX
-            usleep(10000);
-#endif
-#ifdef Q_OS_WIN32
-            boost::this_thread::sleep(boost::posix_time::milliseconds(10));
-#endif
-             std::cout<<"enter mbRunPause"<<std::endl;
-            continue;
-
-        }
-
-
-
-        ServiceCarStatus.mbBrainCtring = true;
-        obs_lidar_ = obs_radar_ = NULL;
-
-        int j;
-        //      gps_data_ = NULL;
-
-        //        if(obs_lidar_grid_ != NULL)free(obs_lidar_grid_);
-        if(obs_lidar_grid_ != NULL)
-        {
-            if(old_obs_lidar_grid_ != NULL) free(old_obs_lidar_grid_);
-            old_obs_lidar_grid_ = obs_lidar_grid_;
-        }
-
-        obs_lidar_grid_ = NULL;
-
-        if(obs_camera_ != NULL)free(obs_camera_);
-
-        obs_camera_ = NULL;
-
-
-
-        eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
-
-
-//        if(obs_lidar_grid_!= NULL)
-//        {
-//            std::cout<<"receive fusion date"<<std::endl;
-//            int i,j;
-//            for(i=0;i<iv::grx;i++)
-//            {
-//                for(j=0;j<iv::gry;j++)
-//                {
-//                 if(obs_lidar_grid_[i*iv::gry+j].ob == 2){
-//                     std::cout<<"ok"<<std::endl;
-//                     if(obs_lidar_grid_[i*iv::gry+j].id > 10)
-//                     {
-//                         int xx = obs_lidar_grid_[i*iv::gry+j].id;
-//                         xx++;
-//                     }
-//                      std::cout<<obs_lidar_grid_[i*iv::gry+j].id<<std::endl;
-//                 }
-
-
-//                }
-//            }
-//              std::cout<<"print fusion date end"<<std::endl;
-//        }
-
-
-        //ServiceLidar.copylidarperto(lidar_per);
-
-        //        if(lidar_per->size() >0)
-        //        {
-        //            int i;
-        //            for(i=0;i<lidar_per->size();i++)
-        //            {
-        //                if(lidar_per->at(i).label>0)
-        //                {
-        //                    std::cout<<"label is"<<lidar_per->at(i).label<<" vy = "<<lidar_per->at(i).velocity.y<<std::endl;
-        //                }
-        //            }
-        //            //if size > 0, have perception result;
-        //        }
-
-        iv::ObsLiDAR obs_lidar_tmp(new std::vector<iv::ObstacleBasic>);
-        iv::ObsRadar obs_radar_tmp(new std::vector<iv::ObstacleBasic>);
-        iv::decition::Decition decition_final(new iv::decition::DecitionBasic);
-
-        //		if ((ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == false && ServiceCarStatus.status[4] == true) ||
-        //			(ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true) ||
-        //			(ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == true && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
-        //		{
-        //			//停车
-        //			ServiceCarStatus.carState = 0;
-        //		}
-        //		else if ((ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == false && ServiceCarStatus.status[4] == true) ||
-        //				 (ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
-        //		{
-        //			//正常循迹
-        //			ServiceCarStatus.carState = 1;
-        //		}
-        //		else if ((ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true) ||
-        //				 (ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == true && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
-        //		{
-        //			//前往站点
-        //			ServiceCarStatus.carState = 2;
-        //		}
-
-        if (obs_lidar_) {	//激光雷达有障碍物
-            //          std::cout<<"obs lidar size = "<<obs_lidar_->size()<<std::endl;
-            //std::cout << "大脑处理激光雷达:" << obs_lidar_->at(0).nomal_x << " " << obs_lidar_->at(0).nomal_y << " " << obs_lidar_->at(0).nomal_z << std::endl;
-            obs_lidar_tmp = NULL;
-            obs_lidar_tmp.swap(obs_lidar_);
-        }
-        else
-        {
-            //           std::cout<<"no lidar obs"<<std::endl;
-        }
-        //ServiceCarStatus.obs_radar;
-        //毫米波雷达障碍物信息
-
-        if (obs_radar_) {
-            //std::cout << "大脑处理毫米波雷达:" << obs_radar_->at(0).nomal_x << " " << obs_radar_->at(0).nomal_y << " " << obs_radar_->at(0).nomal_z << std::endl;
-            obs_radar_tmp = NULL;
-            obs_radar_tmp.swap(obs_radar_);
-        }
-        if (obs_camera_) {
-            //std::cout << "大脑处理摄像头雷达:" << obs_camera_->at(0).nomal_x << " " << obs_camera_->at(0).nomal_y << " " << obs_camera_->at(0).nomal_z << std::endl;
-        }
-
-
-        //useMobileEye chuku
-        if(ServiceCarStatus.m_bOutGarage){
-            GPS_INS gps ;
-            if(gps_data_){
-                gps=*gps_data_;
-            }
-            decition_gps = decitionLine00->getDecideFromGPS(gps, navigation_data, obs_lidar_grid_,*lidar_per);  //chedaoxian
-
-            std::cout << "使用mobileEye决策"<< std::endl;
-        }
-        //useMobileEye chuku end;
-
-        if (gps_data_  &&!handPark && !ServiceCarStatus.m_bOutGarage){// && obs_lidar_grid_) {	//处理GPS数据
-            //todo gps_data_为当前读到的gps位置信息
-            //decition_gps = iv::decition::Decition(new iv::decition::DecitionBasic);
-            //ODS("接收GPS数据:%f\t\t%f\n", gps_data_->gps_x, gps_data_->gps_y);
-            //	decition_gps = decitionMaker->getDecideForGPS(*gps_data_,navigation_data);
-
-            //ODS("\n接收到的实时GPS数据:%f\t%f\t%f\n", gps_data_->gps_x, gps_data_->gps_y, gps_data_->ins_heading_angle);
-
-            //以下为正常用
-            //         if (gps_data_->valid == RTK_IMU_OK)
-            //        {
-            double start = GetTickCount();
-
-            //				decitionGps00->aim_gps_ins.gps_lat = carcall->lat;
-            //				decitionGps00->aim_gps_ins.gps_lng = carcall->lon;
-            /*iv::ObsRadar obs_radar_tmp(new std::vector<iv::ObstacleBasic>);
-                if(obs_radar_){
-                    obs_radar_tmp = NULL;
-                    obs_radar_tmp.swap(obs_radar_);
-                }*/
-            //decition_gps = decitionGpsUnit->getDecideFromGPS(*gps_data_, navigation_data, *obs_radar_tmp, obs_lidar_grid_);
-            //		decition_gps = decitionGpsUnit->getDecideFromGPS(*gps_data_, navigation_data, obs_lidar_grid_);     // 新的
-
-
-            if ( ServiceCarStatus.epsMode==0 && ServiceCarStatus.receiveEps) {
-                ServiceCarStatus.mbRunPause=true;
-
-            }
-            ServiceCarStatus.receiveEps=false;
-
-            lastMode=ServiceCarStatus.epsMode;
-            lastPause=ServiceCarStatus.mbRunPause;
-
-            if((fabs(mTime.elapsed() - mnOldTime)>40))
-            {
-                if((fabs(mTime.elapsed() - mnOldTime)<10000))
-                {
-                    ServiceCarStatus.mfCalAcc = (ServiceCarStatus.speed - mfSpeedLast)*1000/(fabs(mTime.elapsed() - mnOldTime))/3.6;
-                    ServiceCarStatus.mfVehAcc = (ServiceCarStatus.vehSpeed_st - mfVehSpeedLast)*1000/(fabs(mTime.elapsed() - mnOldTime))/3.6;
-                }else
-                {
-                    ServiceCarStatus.mfCalAcc = 0;
-                    ServiceCarStatus.mfVehAcc = 0;
-                }
-                mnOldTime = mTime.elapsed();
-                mfSpeedLast = ServiceCarStatus.speed;
-                mfVehSpeedLast=ServiceCarStatus.vehSpeed_st;
-            }
-
-            iv::LidarGridPtr templidar= obs_lidar_grid_;
-            if(templidar == NULL)templidar = old_obs_lidar_grid_;
-        //    GaussProjCal(gps_data_->gps_lng,gps_data_->gps_lat,&gps_data_->gps_x,&gps_data_->gps_y);
-
-            mMutexMap.lock();
-            decition_gps = decitionGps00->getDecideFromGPS(*gps_data_, navigation_data, templidar,*lidar_per,trafficLight);  // my dicition=============================
-            mMutexMap.unlock();
-
-            if(mbUseExternMPC)
-            {
-                fanya::GPS_INS xgpsins;
-                xgpsins.speed_y = gps_data_->speed/3.6;
-                xgpsins.gps_lat = gps_data_->gps_lat;
-                xgpsins.gps_lng = gps_data_->gps_lng;
-                xgpsins.ins_heading = gps_data_->ins_heading_angle;
-                mmpcapi.SetGPS(xgpsins);
-                mmpcapi.SetDesiredspeed(decition_gps->speed/3.6);
-                double mpc_speed,mpc_decision,mpc_wheel;
-                if(mmpcapi.GetDecision(mpc_speed,mpc_decision,mpc_wheel) == 0)
-                {
-                    decition_gps->wheel_angle = mpc_wheel;
-                }
-            }
-            iv::modulecomm::ModuleSendMsg(mpaPlanTrace,(char *)decitionGps00->planTrace.data(),decitionGps00->planTrace.size()*sizeof(iv::Point2D));
-
-
-
-            //      decition_gps = decitionLine00->getDecideFromGPS(*gps_data_, navigation_data, obs_lidar_grid_);
-            //ODS("\n决策：%f\t%f\t\t", decition_gps->speed, decition_gps->wheel_angle);
-            //ODS("\n接收到的实时GPS数据:%f\t%f\t%f\n", gps_data_->gps_x, gps_data_->gps_y,gps_data_->ins_heading_angle);
-
-            //carcall->is_arrived = decitionGps00->is_arrivaled;
-            //double end = GetTickCount();
-            decision_period = start - last;
-            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n周期时长:%f\n" << decision_period << std::endl;
-            //                iv::decition::VehicleStateBasic vsb;
-            //                vsb.mbBocheEnable = ServiceCarStatus.bocheEnable;
-            //                vsb.mfLidarObs = ServiceCarStatus.mLidarObs;
-            //                vsb.mfRadarObs = ServiceCarStatus.mRadarObs;
-            //                vsb.mfObs = ServiceCarStatus.mRadarObs;
-            //                vsb.decision_period = decision_period;
-            //                vsb.mbBrainRunning = ServiceCarStatus.mbBrainCtring;
-            ////                mpaVechicleState->writemsg((char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
-
-            //                iv::modulecomm::ModuleSendMsg(mpaVechicleState,(char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
-
-            iv::brain::brainstate xbs;
-            xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
-            xbs.set_mbbrainrunning(true);  //apollo_fu debug ui 20200417
-            xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
-            xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
-            xbs.set_mfobs(ServiceCarStatus.mObs);
-            xbs.set_decision_period(decision_period);
-            ShareBrainstate(xbs);
-
-
-            //                decitionExecuter->executeDecition(decition_gps);
-            ShareDecition(decition_gps);
-
-            givlog->debug("acc is %f brake is %f wheel is %f",
-                          decition_gps->accelerator,decition_gps->brake,
-                          decition_gps->wheel_angle);
-            givlog->debug("period id %f",decision_period);
-
-            //	ODS("\n周期时长:%f\n", start - last);
-            //	ODS("\n决策时长:%f\n", end - start);
-            //	ODS("\n接收到的实时GPS车速:%f\n", gps_data_->speed);
-            //	ODS("\n接收到的实时GPS车y轴加速:%f\n", gps_data_->accel_y);
-            //	ODS("\n接收到的实时GPS车x轴加速:%f\n", gps_data_->accel_x);
-            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策加速度:%f\n" << decition_gps->accelerator << std::endl;
-            //		ODS("\n决策刹车:%f\n", decition_gps->brake);
-            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策方向:%f\n" << decition_gps->wheel_angle << std::endl;
-            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策速度:%f\n" << decition_gps->speed << std::endl;
-            last = start;
-            //decitionMaker->decideFromGPS(decition_gps, gps_data_, navigation_data);		//gps_data_为当前读到的gps位置信息  navigation_data为导航数据  decition_gps为根据前两者得出的决策速度与决策角度
-            //            }
-
-        }
-
-        if (handPark && decition_gps != NULL)
-        {
-            decition_gps->brake = 20;
-            decition_gps->accelerator = 0;
-            //			decitionExecuter->executeDecition(decition_gps);
-            ShareDecition(decition_gps);
-            //boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
-
-#ifdef linux
-            usleep(2000000);
-#endif
-#ifdef WIN32
-            boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
-            //           Sleep(2000);
-#endif
-            decition_gps->brake = 0;
-            decition_gps->accelerator = 0;
-            //			decitionExecuter->executeDecition(decition_gps);
-            //			ServiceCanUtil.set_handbrake_on();
-            //boost::this_thread::sleep(boost::posix_time::milliseconds(handParkTime));
-            ShareDecition(decition_gps);
-
-#ifdef linux
-            usleep(handParkTime*1000);
-#endif
-#ifdef WIN32
-            Sleep(handParkTime);
-#endif
-            handPark = false;
-        }
-
-
-        else
-        {
-            //boost::this_thread::sleep(boost::posix_time::milliseconds(1));
-
-#ifdef linux
-            usleep(1000);
-#endif
-#ifdef WIN32
-            Sleep(1);
-#endif
-            //			ODS("no gps data .sleep.\r\n");
-        }
-
-
-
-
-        //        if (decition_lidar || decition_radar || decition_camera || decition_gps || decition_lidar) {
-        //            //	decitionVoter->decideFromAll(decition_final, decition_lidar, decition_radar, decition_camera, decition_gps, decition_localmap);
-        //            //	decitionExecuter->executeDecition(decition_final);	//执行最终的决策
-        //            /*	decition_gps->brake = 0;
-        //            decition_gps->accelerator = 0;*/
-        //            look1 = decition_gps->accelerator;
-        //            look2 = decition_gps->brake;
-        //            look3 = decition_gps->wheel_angle;
-        //            look4 = decition_gps->speed;
-        //            look5 = decitionGps00->lidarDistance;
-        //            look6 = decitionGps00->myesrDistance;
-        //            look7 = decitionGps00->obsDistance;
-
-        //            decition_gps->grade=1;
-        //            decition_gps->mode=1;
-        //            decition_gps->speak=0;
-        //            decition_gps->handBrake=0;
-        //            decition_gps->bright=false;
-        //            decition_gps->engine=0;
-
-        //            if(ServiceCarStatus.bocheMode){
-        //                decition_gps->dangWei=2;
-        //            }else{
-        //                decition_gps->dangWei=1;
-        //            }
-
-
-        //            decition_gps->wheel_angle=max((float)-500.0,float(decition_gps->wheel_angle+ServiceCarStatus.mfEPSOff));
-        //            decition_gps->wheel_angle=min((float)500.0,decition_gps->wheel_angle);
-
-        //            //			decitionExecuter->executeDecition(decition_gps);
-        //            //           ShareDecition(decition_gps);
-        //        }
-
-        ServiceCarStatus.mfAcc = decition_gps->accelerator;
-        ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
-        ServiceCarStatus.mfBrake = decition_gps->brake;
-
-        iv::platform::PlatFormMsg toplat;
-        toplat.carState = ServiceCarStatus.carState;	// 0:停车	1:正常循迹	2:前往站点
-        toplat.istostation = ServiceCarStatus.istostation;
-        toplat.currentstation = ServiceCarStatus.currentstation;
-        toplat.clientto = ServiceCarStatus.clientto;
-        toplat.amilng = ServiceCarStatus.amilng;
-        toplat.amilat = ServiceCarStatus.amilat;
-        //       mpaToPlatform->writemsg((char*)&toplat,sizeof(iv::platform::PlatFormMsg));
-
-        iv::modulecomm::ModuleSendMsg(mpaToPlatform,(char*)&toplat,sizeof(iv::platform::PlatFormMsg));
-        //ODS("\ngetSensor时长:%f\n", end1 - start1);
-    }
-    std::cout << "\n\n\n\n\n\n\n\n\n\n\nbrain线程退出\n" << std::endl;
-}
-
-bool iv::decition::BrainDecition::loadMapFromFile(std::string fileName) {
-    std::ifstream fis(fileName);//获取文件
-    std::string line;
-    std::vector<std::string> des;
-
-    if (fis.is_open() == false)
-        return false;
-    try {
-        while (std::getline(fis, line)) {//开始一行一行的读数据
-            boost::split(des, line, boost::is_any_of("\t"));
-            if (des.size() != 10)
-                throw "error";
-            iv::GPSData data(new iv::GPS_INS);
-            data->index = atoi(des[0].c_str());
-            data->gps_lng = atof(des[1].c_str());
-            data->gps_lat = atof(des[2].c_str());
-            data->speed_mode = atoi(des[3].c_str());
-            data->mode2 = atoi(des[4].c_str());
-            data->ins_heading_angle = atof(des[5].c_str());
-            data->runMode = atoi(des[6].c_str());
-            data->roadMode = atoi(des[7].c_str());
-            data->roadSum = atoi(des[8].c_str());
-            data->roadOri = atoi(des[9].c_str());
-            //      gps2xy(data->gps_lat, data->gps_lng, &data->gps_x, &data->gps_y);
-            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-            //data->speed = 5;	//调试用  所有点速度都设为5km/h
-            //LOG2(data->gps_x, data->gps_y);
-            //ODS("x %.20lf   y %.20lf\n", data->gps_x, data->gps_y);
-
-            //	iv::decition::BLH2XYZ(*data);   //重新转下大地坐标
-
-            if(data->roadMode==4){
-                data->ins_heading_angle=data->ins_heading_angle+180;
-                if(data->ins_heading_angle>=360)
-                    data->ins_heading_angle=data->ins_heading_angle-360;
-            }
-
-            navigation_data.push_back(data);
-        }
-
-    }
-    catch (...) {
-        fis.close();
-        return false;
-    }
-
-    fis.close();
-    return true;
-}
-
-bool iv::decition::BrainDecition::loadMapFromFile2(std::string fileName) {
-    std::ifstream fis(fileName);//获取文件
-    std::string line;
-    std::vector<std::string> des;
-
-    if (fis.is_open() == false)
-        return false;
-    try {
-        while (std::getline(fis, line)) {//开始一行一行的读数据
-            boost::split(des, line, boost::is_any_of("\t"));
-            if (des.size() != 10)
-                throw "error";
-            iv::GPSData data(new iv::GPS_INS);
-            data->index = atoi(des[0].c_str());
-            data->gps_lng = atof(des[1].c_str());
-            data->gps_lat = atof(des[2].c_str());
-            data->speed_mode = atoi(des[3].c_str());
-            data->mode2 = atoi(des[4].c_str());
-            data->ins_heading_angle = atof(des[5].c_str());
-            data->runMode = atoi(des[6].c_str());
-            data->roadMode = atoi(des[7].c_str());
-            data->roadSum = atoi(des[8].c_str());
-            data->roadOri = atoi(des[9].c_str());
-            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-            navigation_data2.push_back(data);
-        }
-    }
-    catch (...) {
-        fis.close();
-        return false;
-    }
-
-    fis.close();
-    return true;
-}
-
-bool iv::decition::BrainDecition::loadMapFromFile3(std::string fileName) {
-    std::ifstream fis(fileName);//获取文件
-    std::string line;
-    std::vector<std::string> des;
-
-    if (fis.is_open() == false)
-        return false;
-    try {
-        while (std::getline(fis, line)) {//开始一行一行的读数据
-            boost::split(des, line, boost::is_any_of("\t"));
-            if (des.size() != 10)
-                throw "error";
-            iv::GPSData data(new iv::GPS_INS);
-            data->index = atoi(des[0].c_str());
-            data->gps_lng = atof(des[1].c_str());
-            data->gps_lat = atof(des[2].c_str());
-            data->speed_mode = atoi(des[3].c_str());
-            data->mode2 = atoi(des[4].c_str());
-            data->ins_heading_angle = atof(des[5].c_str());
-            data->runMode = atoi(des[6].c_str());
-            data->roadMode = atoi(des[7].c_str());
-            data->roadSum = atoi(des[8].c_str());
-            data->roadOri = atoi(des[9].c_str());
-            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-            navigation_data3.push_back(data);
-        }
-    }
-    catch (...) {
-        fis.close();
-        return false;
-    }
-
-    fis.close();
-    return true;
-}
-
-bool iv::decition::BrainDecition::loadMapFromFile4(std::string fileName) {
-    std::ifstream fis(fileName);//获取文件
-    std::string line;
-    std::vector<std::string> des;
-
-    if (fis.is_open() == false)
-        return false;
-    try {
-        while (std::getline(fis, line)) {//开始一行一行的读数据
-            boost::split(des, line, boost::is_any_of("\t"));
-            if (des.size() != 10)
-                throw "error";
-            iv::GPSData data(new iv::GPS_INS);
-            data->index = atoi(des[0].c_str());
-            data->gps_lng = atof(des[1].c_str());
-            data->gps_lat = atof(des[2].c_str());
-            data->speed_mode = atoi(des[3].c_str());
-            data->mode2 = atoi(des[4].c_str());
-            data->ins_heading_angle = atof(des[5].c_str());
-            data->runMode = atoi(des[6].c_str());
-            data->roadMode = atoi(des[7].c_str());
-            data->roadSum = atoi(des[8].c_str());
-            data->roadOri = atoi(des[9].c_str());
-            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
-            navigation_data4.push_back(data);
-        }
-    }
-    catch (...) {
-        fis.close();
-        return false;
-    }
-
-    fis.close();
-    return true;
-}
-
-void iv::decition::BrainDecition::start() {
-    thread_run = new boost::thread(boost::bind(&iv::decition::BrainDecition::run, this));
-}
-
-void iv::decition::BrainDecition::stop() {
-    //    carcall->stop();
-    thread_run->interrupt();
-    thread_run->join();
-}
-
-void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapdatasize)
-{
-    //    std::cout<<"update map "<<std::endl;
-    int gpsunitsize = sizeof(iv::GPS_INS);
-    int nMapSize = mapdatasize/gpsunitsize;
-    //    std::cout<<"map size is "<<nMapSize<<std::endl;
-
-    if(nMapSize < 1)return;
-
-    bool bUpdate = false;
-    if(nMapSize != navigation_data.size())
-    {
-        bUpdate = true;
-    }
-    else
-    {
-        iv::GPS_INS * p = (iv::GPS_INS *)mapdata;
-        if((p->gps_lat == navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == navigation_data.at(0)->ins_heading_angle))
-        {
-            //           qDebug("same map");
-            bUpdate = false;
-        }
-        else
-        {
-            bUpdate = true;
-        }
-    }
-
-    if(bUpdate)
-    {
-        std::vector<fanya::MAP_DATA> xvectorMAP;
-        int i;
-        mMutexMap.lock();
-        navigation_data.clear();
-        for(i=0;i<nMapSize;i++)
-        {
-            iv::GPS_INS x;
-            memcpy(&x,mapdata + i*gpsunitsize,gpsunitsize);
-            iv::GPSData data(new iv::GPS_INS);
-            *data = x;
-            navigation_data.push_back(data);
-
-            fanya::MAP_DATA xmapdata;
-            xmapdata.gps_lat = x.gps_lat;
-            xmapdata.gps_lng = x.gps_lng;
-            xmapdata.ins_heading = x.ins_heading_angle;
-            xvectorMAP.push_back(xmapdata);
-        }
-
-        mmpcapi.SetMAP(xvectorMAP);
-
-        if(ServiceCarStatus.speed_control==true){
-        Compute00().getDesiredSpeed(navigation_data);}
-        mMutexMap.unlock();
-        //        mpasd->SaveState("map",mapdata,mapdatasize);
-    }
-    else
-    {
-        //       qDebug("not need update");
-    }
-}
-
-void iv::decition::BrainDecition::ShareDecition(iv::decition::Decition decition)
-{
-    iv::brain::decition xdecition;
-    xdecition.set_accelerator(decition->accelerator);
-    xdecition.set_brake(decition->brake);
-    xdecition.set_leftlamp(decition->leftlamp);
-    xdecition.set_rightlamp(decition->rightlamp);
-    xdecition.set_speed(decition->speed);
-    xdecition.set_wheelangle(decition->wheel_angle);
-    xdecition.set_wheelspeed(decition->angSpeed);
-    xdecition.set_torque(decition->torque);
-    xdecition.set_mode(decition->mode);
-    xdecition.set_gear(decition->dangWei);
-    xdecition.set_handbrake(decition->handBrake);
-    xdecition.set_grade(decition->grade);
-    xdecition.set_engine(decition->engine);
-    xdecition.set_brakelamp(decition->brakeLight);
-    xdecition.set_ttc(ServiceCarStatus.mfttc);
-    xdecition.set_air_enable(decition->air_enable);
-    xdecition.set_air_temp(decition->air_temp);
-    xdecition.set_air_mode(decition->air_mode);
-    xdecition.set_wind_level(decition->wind_level);
-    xdecition.set_roof_light(decition->roof_light);
-    xdecition.set_home_light(decition->home_light);
-    xdecition.set_air_worktime(decition->air_worktime);
-    xdecition.set_air_offtime(decition->air_offtime);
-    xdecition.set_air_on(decition->air_on);
-
-
-    std::cout<<"===================shareDecition========================"<<std::endl;
-         std::cout<<"  torque_st:"<<ServiceCarStatus.torque_st
-                 <<"  decideTorque:"<<decition->torque <<"  decideBrake:"<<decition->brake
-                <<"  decition mode:"<<decition->mode
-        <<std::endl;
-    std::cout<<"========================================="<<std::endl;
-
-    givlog->verbose("torque_st: %ld",ServiceCarStatus.torque_st);
-     givlog->verbose("decideTorque: %ld",decition->torque);
-     givlog->verbose("decideBrake: %ld", decition->brake);
-     givlog->debug("wheelAng: %f",decition->wheel_angle);
-
-   //  xdecition.set_wheelangle(100);
-    static qint64 oldtime;
-
-    if((QDateTime::currentMSecsSinceEpoch() - oldtime)>100)
-    {
-        givlog->warn("brain interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldtime);
-    }
-
-    givlog->verbose("decition time is %ld",QDateTime::currentMSecsSinceEpoch());
-    oldtime = QDateTime::currentMSecsSinceEpoch();
-
-
-    givlog->verbose("torque %f wheel %f dangwei %d ",decition->torque,
-                    decition->wheel_angle,decition->dangWei);
-    int nsize = xdecition.ByteSize();
-    char * str = new char[nsize];
-    std::shared_ptr<char> pstr;
-    pstr.reset(str);
-    if(xdecition.SerializeToArray(str,nsize))
-    {
-        if(ServiceCarStatus.mbRunPause == false)
-        {
-
-            iv::modulecomm::ModuleSendMsg(mpaDecition,str,nsize);
-        }
-    }
-    else
-    {
-        std::cout<<"iv::decition::BrainDecition::ShareDecition serialize error."<<std::endl;
-    }
-}
-
-void iv::decition::BrainDecition::ShareBrainstate(iv::brain::brainstate xbs)
-{
-    int nsize = xbs.ByteSize();
-    char * str = new char[nsize];
-    std::shared_ptr<char> pstr;
-    pstr.reset(str);
-    if(xbs.SerializeToArray(str,nsize))
-    {
-        iv::modulecomm::ModuleSendMsg(mpaVechicleState,str,nsize);
-    }
-    else
-    {
-        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
-    }
-}
-
-void iv::decition::BrainDecition::UpdateHMI(const char *pdata, const int ndatasize)
-{
-    if(ndatasize < sizeof(iv::hmi::HMIBasic))return;
-
-    iv::hmi::hmimsg xhmimsg;
-    if(!xhmimsg.ParseFromArray(pdata,ndatasize))
-    {
-        givlog->error("iv::decition::BrainDecition::UpdateHMI parse error");
-        return;
-    }
-
-    ServiceCarStatus.mbRunPause = xhmimsg.mbpause();
-
-    if(xhmimsg.mbbochemode()){
-         ServiceCarStatus.bocheMode = true;
-    }
-    ServiceCarStatus.busmode = xhmimsg.mbbusmode();
-
-    if( ServiceCarStatus.bocheMode ){
-        int a=0;
-    }
-
-    bool bRun;
-    bRun = !ServiceCarStatus.mbRunPause;
-
-    //    mpasd->SaveState("runstate",(char *)&bRun,sizeof(bool));
-
-}
-
-void iv::decition::BrainDecition::UpdatePlatform(const char *pdata, const int ndatasize)
-{
-    if(ndatasize < sizeof(iv::platform::PlatFormMsg))return;
-
-    iv::platform::PlatFormMsg x;
-    memcpy(&x,pdata,sizeof(iv::platform::PlatFormMsg));
-    ServiceCarStatus.carState = x.carState;	// 0:停车	1:正常循迹	2:前往站点
-    std::cout<<"car state "<<ServiceCarStatus.carState<<std::endl;
-    ServiceCarStatus.istostation = x.istostation;
-    ServiceCarStatus.currentstation = x.currentstation;
-    ServiceCarStatus.clientto = x.clientto;
-    ServiceCarStatus.amilng = x.amilng;
-    ServiceCarStatus.amilat = x.amilat;
-
-}
-
-void iv::decition::BrainDecition::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
-{
-    iv::chassis xchassis;
-    static int ncount = 0;
-    if(!xchassis.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
-        return;
-    }
-
-    ServiceCarStatus.epb = xchassis.epbfault();
-    ServiceCarStatus.grade = xchassis.shift();
-    ServiceCarStatus.driverMode = xchassis.drivemode();
-    if(xchassis.has_epsmode()){
-        ServiceCarStatus.epsMode = xchassis.epsmode();
-        ServiceCarStatus.receiveEps=true;
-        if(ServiceCarStatus.epsMode == 0)
-        {
-            ServiceCarStatus.mbRunPause = true;
-        }
-    }
-    if(xchassis.has_torque())
-    {
-         ServiceCarStatus.torque_st = xchassis.torque();
-        if(ServiceCarStatus.msysparam.mvehtype=="bus"){
-            ServiceCarStatus.torque_st = xchassis.torque()*0.4;
-        }
-        std::cout<<"******* ServiceCarStatus.torque_st:"<< ServiceCarStatus.torque_st<<std::endl;
-        givlog->warn(" ServiceCarStatus.torque_st: is %f",ServiceCarStatus.torque_st);
-
-    }
-    //    if(xchassis.epsmode() == 1)
-    //    {
-    //        ncount++;
-    ////        if(ncount > 10)ServiceCarStatus.mbRunPause = true;
-    //    }
-    //    else
-    //    {
-    //        ncount = 0;
-    //    }
-}
-
-void iv::decition::BrainDecition::UpdateGroupInfo(const char *pdata, const int ndatasize){
-
-    iv::radio::radio_send group_message;
-    if(false == group_message.ParseFromArray(pdata,ndatasize))
-    {
-        std::cout<<"Listencansend Parse fail."<<std::endl;
-        return;
-    }
-    ServiceCarStatus.group_server_status = group_message.server_status();
-    ServiceCarStatus.group_comm_speed= (float)group_message.car_control_speed();
-    ServiceCarStatus.group_state= group_message.cmd_mode();
-
-    qDebug("updateGroupInfo %ld",QDateTime::currentMSecsSinceEpoch());
-    if(ServiceCarStatus.group_state!=2){
-        ServiceCarStatus.group_comm_speed=0;
-    }
-    qDebug("serverSt: %d, speed: %f, state: %d", ServiceCarStatus.group_server_status, ServiceCarStatus.group_comm_speed, ServiceCarStatus.group_state);
-
-
-}
-
-void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatasize){
-
-    iv::vbox::vbox group_message;
-    if(false == group_message.ParseFromArray(pdata,ndatasize))
-    {
-        std::cout<<"Listencansend Parse fail."<<std::endl;
-        return;
-    }
-
-//  解析box信息
-//    ServiceCarStatus.group_server_status = group_message.server_status();
-//    ServiceCarStatus.group_comm_speed= (float)group_message.car_control_speed();
-//    ServiceCarStatus.group_state= group_message.cmd_mode();
-
-
-    trafficLight.leftColor=group_message.st_left();
-    trafficLight.rightColor=group_message.st_right();
-    trafficLight.straightColor=group_message.st_straight();
-    trafficLight.uturnColor=group_message.st_turn();
-    trafficLight.leftTime=group_message.time_left();
-    trafficLight.rightTime=group_message.time_right();
-    trafficLight.straightTime=group_message.time_straight();
-    trafficLight.uturnTime=group_message.time_turn();
-
-
-
-}
-
-void iv::decition::BrainDecition::UpdateSate(){
-     decitionGps00->isFirstRun=true;
-}
+#include <decition/brain.h>
+#include <fstream>
+#include <time.h>
+#include <exception>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <common/logout.h>
+#include <decition/adc_tools/transfer.h>
+
+#include <iostream>
+#include "xmlparam.h"
+#include "qcoreapplication.h"
+
+
+extern std::string gstrmemdecition;
+extern std::string gstrmembrainstate;
+extern iv::Ivlog * givlog;
+extern std::string gstrmemchassis;
+
+#define LOG_ENABLE
+
+namespace iv {
+namespace decition {
+iv::decition::BrainDecition * gbrain;
+
+
+        void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateMap(strdata,nSize);
+            gbrain->UpdateSate();
+        }
+
+        void ListenVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateVbox(strdata,nSize);
+            gbrain->UpdateSate();
+        }
+
+        void ListenHMI(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateHMI(strdata,nSize);
+        }
+
+        void ListenPlatform(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdatePlatform(strdata,nSize);
+        }
+
+        void ListenGroup(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateGroupInfo(strdata,nSize);
+        }
+
+    /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            iv::formation_map_index::map map;
+            if(false == map.ParseFromArray(strdata,nSize))
+            {
+                givlog->error("GPS","[%s] Listencansend Parse fail.",__func__);
+                return;
+            }else{
+               gbrain->UpdateSate();
+            }
+        //    map.index()-1  map index num.
+        }*/
+
+    }
+
+}
+
+
+
+iv::decition::BrainDecition::BrainDecition()
+{
+    //    mpasd = new Adcivstatedb();
+    look1 = 0.0;
+    look2 = 0.0;
+    look3 = 0.0;
+    look4 = 0.0;
+    look5 = 0.0;
+    look6 = 0.0;
+    look7 = 0.0;
+    obs_lidar_grid_ = NULL;
+    old_obs_lidar_grid_ = NULL;
+    obs_camera_ = NULL;
+    obs_fusion_grid_ = NULL;
+    //    controller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());
+
+    //    carcall = new iv::carcalling::carcalling();
+    eyes = new iv::perception::Eyes();
+    //	decitionMaker = new iv::decition::DecitionMaker();
+
+    decitionGps00 = new iv::decition::DecideGps00();
+    decitionLine00=new iv::decition::DecideLine00();
+
+    //	decitionGpsUnit = new iv::decition::DecideGpsUnit();
+
+    //	decitionExecuter = new iv::decition::DecitionExecuter(controller);
+
+    iv::decition::gbrain = this;
+
+
+    mpa = iv::modulecomm::RegisterRecv("tracemap",iv::decition::ListenTraceMap);
+
+    mpvbox = iv::modulecomm::RegisterRecv("vbox",iv::decition::ListenVbox);
+
+    mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
+
+
+    mpaToPlatform = iv::modulecomm::RegisterSend("toplatform",10*sizeof(iv::platform::PlatFormMsg),10);
+    mpaDecition = iv::modulecomm::RegisterSend(gstrmemdecition.data(),10*sizeof(iv::decition::DecitionBasic),1);
+
+    mpaHMI = iv::modulecomm::RegisterRecv("hmi",iv::decition::ListenHMI);
+
+    mpaPlatform = iv::modulecomm::RegisterRecv("platformfrom",iv::decition::ListenPlatform);
+
+    mpaGroup = iv::modulecomm::RegisterRecv("p900_send",iv::decition::ListenGroup);
+ //   iv::modulecomm::RegisterRecv("map_index",ListenMap_change_req);
+
+
+
+
+    mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
+
+    ModuleFun funchassis =std::bind(&iv::decition::BrainDecition::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    void * pa = iv::modulecomm::RegisterRecvPlus(gstrmemchassis.data(),funchassis);
+
+    mpaPlanTrace = iv::modulecomm::RegisterSend("plantrace",100000,1);
+
+    mTime.start();
+    mnOldTime = mTime.elapsed();
+
+}
+
+iv::decition::BrainDecition::~BrainDecition()
+{
+}
+
+
+void iv::decition::BrainDecition::inialize() {
+    //	controller->inialize();
+    eyes->inialize();
+    //    carcall->start();
+}
+
+
+void iv::decition::BrainDecition::run() {
+
+    double last = 0;
+
+    iv::decition::Decition decition_gps = NULL;
+    iv::decition::Decition decition_lidar = NULL;
+    iv::decition::Decition decition_radar = NULL;
+    iv::decition::Decition decition_camera = NULL;
+    iv::decition::Decition decition_localmap = NULL;
+
+    iv::decition::Decition dgtemp(new iv::decition::DecitionBasic);
+    decition_gps = dgtemp;
+    decition_gps->brake = 0;
+    decition_gps->accelerator = 0;
+    decition_gps->wheel_angle = 0;
+
+    //controller->auto_drive_mode_enable(true);
+
+    /*Decition test(new DecitionBasic);
+    while (true)
+    {
+        test->accelerator = 1.2;
+        test->wheel_angle = 0;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = 0;
+        test->wheel_angle = 0;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = -1.2;
+        test->wheel_angle = 0;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = 0;
+        test->wheel_angle = 45;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = 0;
+        test->wheel_angle = 0;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = 0;
+        test->wheel_angle = -60;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+    }*/
+
+    //   std::cout<<"start run."<<std::endl;
+
+    //   ServiceCarStatus.mbRunPause = false;
+
+    bool bRun;
+    int nValueSize;
+
+    //    if(mpasd->LoadState("runstate",(char *)&bRun,sizeof(bool),&nValueSize) == true)
+    //    {
+    //        if(bRun)
+    //        {
+    //            ServiceCarStatus.mbRunPause = false;
+    //        }
+    //        else
+    //        {
+    //            ServiceCarStatus.mbRunPause = true;
+    //        }
+    //    }
+
+    char * strmap = new char[10000000];
+    int nMapSize;
+
+    //    if(mpasd->LoadState("map",strmap,10000000,&nMapSize) == true)
+    //    {
+    //        UpdateMap(strmap,nMapSize);
+    //    }
+
+    QString strpath = QCoreApplication::applicationDirPath();
+    strpath = strpath + "/ADS_decision.xml";
+    iv::xmlparam::Xmlparam xp(strpath.toStdString());
+    ServiceCarStatus.msysparam.mstrvin = xp.GetParam("vin","10000000000000001");
+    ServiceCarStatus.msysparam.mstrid = xp.GetParam("id","1");
+    ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
+    ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");
+
+
+
+    /*=============     20200113 apollo_fu  add ===========*/
+    std::string str_zhuchetime = xp.GetParam("zhuchetime","16");
+    ServiceCarStatus.msysparam.mzhuchetime =atoi(str_zhuchetime.data());
+    /*============= end ================================== */
+
+    std::string strepsoff = xp.GetParam("epsoff","0.0");
+    ServiceCarStatus.mfEPSOff = atof(strepsoff.data());
+
+    std::string strroadmode_vel = xp.GetParam("roadmode0","10");
+    ServiceCarStatus.mroadmode_vel.mfmode0 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode11","30");
+    ServiceCarStatus.mroadmode_vel.mfmode11 = atof(strroadmode_vel.data());
+
+
+    strroadmode_vel = xp.GetParam("roadmode14","15");
+    ServiceCarStatus.mroadmode_vel.mfmode14 = atof(strroadmode_vel.data());
+
+    //float a = ServiceCarStatus.mroadmode_vel.mfmode14;
+    //cout<<"........"<<a<<endl;
+
+    strroadmode_vel = xp.GetParam("roadmode15","15");
+    ServiceCarStatus.mroadmode_vel.mfmode15 = atof(strroadmode_vel.data());
+
+    /*==================== 20200113    apollo_fu add =================*/
+    strroadmode_vel = xp.GetParam("roadmode5","10");
+    ServiceCarStatus.mroadmode_vel.mfmode5 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode13","5");
+    ServiceCarStatus.mroadmode_vel.mfmode13 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode16","8");
+    ServiceCarStatus.mroadmode_vel.mfmode16 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode17","8");
+    ServiceCarStatus.mroadmode_vel.mfmode17 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode18","5");
+    ServiceCarStatus.mroadmode_vel.mfmode18 = atof(strroadmode_vel.data());
+
+
+
+    /*=========================    end   =============================*/
+
+    std::string group_cotrol = xp.GetParam("group","false");
+    if(group_cotrol=="true"){
+        ServiceCarStatus.group_control=true;
+    }else{
+        ServiceCarStatus.group_control=false;
+    }
+
+    std::string speed_control = xp.GetParam("speed","false");
+    if(speed_control=="true"){
+        ServiceCarStatus.speed_control=true;
+    }else{
+        ServiceCarStatus.speed_control=false;
+    }
+
+
+
+    std::string strparklat = xp.GetParam("parklat","39.0624557");
+    std::string strparklng = xp.GetParam("parklng","117.3575493");
+    std::string strparkheading = xp.GetParam("parkheading","112.5");
+    std::string strparktype = xp.GetParam("parktype","1");
+
+    ServiceCarStatus.mfParkLat = atof(strparklat.data());
+    ServiceCarStatus.mfParkLng = atof(strparklng.data());
+    ServiceCarStatus.mfParkHeading = atof(strparkheading.data());
+    ServiceCarStatus.mnParkType = atoi(strparktype.data());
+
+    ServiceCarStatus.mLidarRotation = atof(xp.GetParam("LidarRotation","90.0").data());
+    ServiceCarStatus.mLidarRangeUnit = atof(xp.GetParam("LidarRangeUnit","5.0").data());
+
+
+
+    std::string lightlon = xp.GetParam("lightlon","0");
+    std::string lightlat = xp.GetParam("lightlat","0");
+    ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data());
+    ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());
+
+
+
+    //mobileEye
+    std::string   timeWidth =xp.GetParam("waitGpsTimeWidth","5000");
+    std::string   garageLong =xp.GetParam("outGarageLong","10");
+    ServiceCarStatus.waitGpsTimeWidth=atof(timeWidth.data());
+    ServiceCarStatus.outGarageLong=atof(garageLong.data());
+    //mobileEye end
+
+
+    //lidar start
+    std::string lidarGpsXiuzheng = xp.GetParam("lidarGpsXiuzheng","1.0");
+    std::string radarGpsXiuzheng = xp.GetParam("radarGpsXiuzheng","3.0");
+    std::string rearRadarGpsXiuzheng = xp.GetParam("rearRadarGpsXiuzheng","0.5");
+    std::string rearLidarGpsXiuzheng = xp.GetParam("rearLidarGpsXiuzheng","1.0");
+    std::string frontGpsXiuzheng = xp.GetParam("frontGpsXiuzheng","3.0");
+    std::string rearGpsXiuzheng = xp.GetParam("rearGpsXiuzheng","1.0");
+    std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
+    std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
+    std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
+    if(strexternmpc == "true")
+    {
+        mbUseExternMPC = true;
+    }
+
+    ServiceCarStatus.msysparam.lidarGpsXiuzheng = atof(lidarGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.radarGpsXiuzheng = atof(radarGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng = atof(rearRadarGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng = atof(rearLidarGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.frontGpsXiuzheng = atof(frontGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.rearGpsXiuzheng = atof(rearGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.gpsOffset_x = atof(gpsOffset_x.data());
+    ServiceCarStatus.msysparam.gpsOffset_y = atof(gpsOffset_y.data());
+    // lidar end
+
+    std::string strObsPredict = xp.GetParam("obsPredict","false");  //If Use MPC set true
+    if(strObsPredict == "true")
+    {
+        ServiceCarStatus.useObsPredict = true;
+    }
+    //map
+
+    mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
+
+
+    while (eyes->isAllSensorRunning())
+    {
+        if(navigation_data.size() <1)
+        {
+            iv::map::mapreq x;
+            x.set_maptype(1);
+            int nsize = x.ByteSize();
+            char * str = new char[nsize];
+            if(x.SerializeToArray(str,nsize))
+            {
+                iv::modulecomm::ModuleSendMsg(mpamapreq,str,nsize);
+            }
+            else
+            {
+                std::cout<<"iv::map::mapreq serialize error."<<std::endl;
+            }
+            delete str;
+        }
+
+
+        iv::GPSData gps_data_;			//gps 惯导数据
+
+        //       qDebug("size = %d",navigation_data.size());
+
+
+
+        std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
+
+        bool brun =ServiceCarStatus.mbRunPause;
+        if(ServiceCarStatus.mnBocheComplete > 0)
+        {
+            ServiceCarStatus.mbBrainCtring = false;
+            ServiceCarStatus.mbRunPause = true;
+            ServiceCarStatus.mnBocheComplete--;
+        }
+        if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
+        {
+            ServiceCarStatus.mbBrainCtring = false;
+            decition_gps->brake = 6;
+            decition_gps->accelerator = -0.5;
+            decition_gps->wheel_angle = 0;
+            decition_gps->torque = 0;
+            decition_gps->mode = 0;
+            //decitionExecuter->executeDecition(decition_gps);
+                   ShareDecition(decition_gps);  //apollo_fu 20200413 qudiaozhushi
+            ServiceCarStatus.mfAcc = decition_gps->accelerator;
+            ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
+            ServiceCarStatus.mfBrake = decition_gps->brake;
+
+            iv::brain::brainstate xbs;
+            xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
+            xbs.set_mbbrainrunning(false);
+            xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
+            xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
+            xbs.set_mfobs(ServiceCarStatus.mObs);
+            xbs.set_decision_period(decision_period);
+            ShareBrainstate(xbs);    //apollo_fu 20200413 qudiaozhushi
+            //            iv::decition::VehicleStateBasic vsb;
+            //            vsb.mbBocheEnable = ServiceCarStatus.bocheEnable;
+            //            vsb.mfLidarObs = ServiceCarStatus.mLidarObs;
+            //            vsb.mfRadarObs = ServiceCarStatus.mRadarObs;
+            //            vsb.mfObs = ServiceCarStatus.mRadarObs;
+            //            vsb.decision_period = decision_period;
+            //            vsb.mbBrainRunning = false;
+            ////             mpaVechicleState->writemsg((char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
+            //            iv::modulecomm::ModuleSendMsg(mpaVechicleState,(char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
+
+#ifdef Q_OS_LINUX
+            usleep(10000);
+#endif
+#ifdef Q_OS_WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(10));
+#endif
+             std::cout<<"enter mbRunPause"<<std::endl;
+            continue;
+
+        }
+
+
+
+        ServiceCarStatus.mbBrainCtring = true;
+        obs_lidar_ = obs_radar_ = NULL;
+
+        int j;
+        //      gps_data_ = NULL;
+
+        //        if(obs_lidar_grid_ != NULL)free(obs_lidar_grid_);
+        if(obs_lidar_grid_ != NULL)
+        {
+            if(old_obs_lidar_grid_ != NULL) free(old_obs_lidar_grid_);
+            old_obs_lidar_grid_ = obs_lidar_grid_;
+        }
+
+        obs_lidar_grid_ = NULL;
+
+        if(obs_camera_ != NULL)free(obs_camera_);
+
+        obs_camera_ = NULL;
+
+
+
+        eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
+
+
+        if(obs_lidar_grid_!= NULL)
+        {
+            std::cout<<"receive fusion date"<<std::endl;
+            int i,j;
+            for(i=0;i<iv::grx;i++)
+            {
+                for(j=0;j<iv::gry;j++)
+                {
+                 if(obs_lidar_grid_[i*iv::gry+j].ob == 2){
+                     std::cout<<"ok"<<std::endl;
+                     if(obs_lidar_grid_[i*iv::gry+j].id > 10)
+                     {
+                         int xx = obs_lidar_grid_[i*iv::gry+j].id;
+                         xx++;
+                     }
+                      std::cout<<obs_lidar_grid_[i*iv::gry+j].id<<std::endl;
+                 }
+
+
+                }
+            }
+              std::cout<<"print fusion date end"<<std::endl;
+        }
+
+
+        //ServiceLidar.copylidarperto(lidar_per);
+
+//                if(lidar_per->size() >0)
+//                {
+//                    int i;
+//                    for(i=0;i<lidar_per->size();i++)
+//                    {
+//                        if(lidar_per->at(i).label>0)
+//                        {
+//                            std::cout<<"label is"<<lidar_per->at(i).label<<" vy = "<<lidar_per->at(i).velocity.y<<std::endl;
+//                        }
+//                    }
+//                    //if size > 0, have perception result;
+//                }
+
+        iv::ObsLiDAR obs_lidar_tmp(new std::vector<iv::ObstacleBasic>);
+        iv::ObsRadar obs_radar_tmp(new std::vector<iv::ObstacleBasic>);
+        iv::decition::Decition decition_final(new iv::decition::DecitionBasic);
+
+        //		if ((ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == false && ServiceCarStatus.status[4] == true) ||
+        //			(ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true) ||
+        //			(ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == true && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
+        //		{
+        //			//停车
+        //			ServiceCarStatus.carState = 0;
+        //		}
+        //		else if ((ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == false && ServiceCarStatus.status[4] == true) ||
+        //				 (ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
+        //		{
+        //			//正常循迹
+        //			ServiceCarStatus.carState = 1;
+        //		}
+        //		else if ((ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true) ||
+        //				 (ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == true && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
+        //		{
+        //			//前往站点
+        //			ServiceCarStatus.carState = 2;
+        //		}
+
+        if (obs_lidar_) {	//激光雷达有障碍物
+            //          std::cout<<"obs lidar size = "<<obs_lidar_->size()<<std::endl;
+            //std::cout << "大脑处理激光雷达:" << obs_lidar_->at(0).nomal_x << " " << obs_lidar_->at(0).nomal_y << " " << obs_lidar_->at(0).nomal_z << std::endl;
+            obs_lidar_tmp = NULL;
+            obs_lidar_tmp.swap(obs_lidar_);
+        }
+        else
+        {
+            //           std::cout<<"no lidar obs"<<std::endl;
+        }
+        //ServiceCarStatus.obs_radar;
+        //毫米波雷达障碍物信息
+
+        if (obs_radar_) {
+            //std::cout << "大脑处理毫米波雷达:" << obs_radar_->at(0).nomal_x << " " << obs_radar_->at(0).nomal_y << " " << obs_radar_->at(0).nomal_z << std::endl;
+            obs_radar_tmp = NULL;
+            obs_radar_tmp.swap(obs_radar_);
+        }
+        if (obs_camera_) {
+            //std::cout << "大脑处理摄像头雷达:" << obs_camera_->at(0).nomal_x << " " << obs_camera_->at(0).nomal_y << " " << obs_camera_->at(0).nomal_z << std::endl;
+        }
+
+
+        //useMobileEye chuku
+        if(ServiceCarStatus.m_bOutGarage){
+            GPS_INS gps ;
+            if(gps_data_){
+                gps=*gps_data_;
+            }
+            decition_gps = decitionLine00->getDecideFromGPS(gps, navigation_data, obs_lidar_grid_,*lidar_per);  //chedaoxian
+
+            std::cout << "使用mobileEye决策"<< std::endl;
+        }
+        //useMobileEye chuku end;
+
+        if (gps_data_  &&!handPark && !ServiceCarStatus.m_bOutGarage){// && obs_lidar_grid_) {	//处理GPS数据
+            //todo gps_data_为当前读到的gps位置信息
+            //decition_gps = iv::decition::Decition(new iv::decition::DecitionBasic);
+            //ODS("接收GPS数据:%f\t\t%f\n", gps_data_->gps_x, gps_data_->gps_y);
+            //	decition_gps = decitionMaker->getDecideForGPS(*gps_data_,navigation_data);
+
+            //ODS("\n接收到的实时GPS数据:%f\t%f\t%f\n", gps_data_->gps_x, gps_data_->gps_y, gps_data_->ins_heading_angle);
+
+            //以下为正常用
+            //         if (gps_data_->valid == RTK_IMU_OK)
+            //        {
+            double start = GetTickCount();
+
+            //				decitionGps00->aim_gps_ins.gps_lat = carcall->lat;
+            //				decitionGps00->aim_gps_ins.gps_lng = carcall->lon;
+            /*iv::ObsRadar obs_radar_tmp(new std::vector<iv::ObstacleBasic>);
+                if(obs_radar_){
+                    obs_radar_tmp = NULL;
+                    obs_radar_tmp.swap(obs_radar_);
+                }*/
+            //decition_gps = decitionGpsUnit->getDecideFromGPS(*gps_data_, navigation_data, *obs_radar_tmp, obs_lidar_grid_);
+            //		decition_gps = decitionGpsUnit->getDecideFromGPS(*gps_data_, navigation_data, obs_lidar_grid_);     // 新的
+
+
+            if ( ServiceCarStatus.epsMode==0 && ServiceCarStatus.receiveEps) {
+                ServiceCarStatus.mbRunPause=true;
+
+            }
+            ServiceCarStatus.receiveEps=false;
+
+            lastMode=ServiceCarStatus.epsMode;
+            lastPause=ServiceCarStatus.mbRunPause;
+
+            if((fabs(mTime.elapsed() - mnOldTime)>40))
+            {
+                if((fabs(mTime.elapsed() - mnOldTime)<10000))
+                {
+                    ServiceCarStatus.mfCalAcc = (ServiceCarStatus.speed - mfSpeedLast)*1000/(fabs(mTime.elapsed() - mnOldTime))/3.6;
+                    ServiceCarStatus.mfVehAcc = (ServiceCarStatus.vehSpeed_st - mfVehSpeedLast)*1000/(fabs(mTime.elapsed() - mnOldTime))/3.6;
+                }else
+                {
+                    ServiceCarStatus.mfCalAcc = 0;
+                    ServiceCarStatus.mfVehAcc = 0;
+                }
+                mnOldTime = mTime.elapsed();
+                mfSpeedLast = ServiceCarStatus.speed;
+                mfVehSpeedLast=ServiceCarStatus.vehSpeed_st;
+            }
+
+            iv::LidarGridPtr templidar= obs_lidar_grid_;
+            if(templidar == NULL)templidar = old_obs_lidar_grid_;
+        //    GaussProjCal(gps_data_->gps_lng,gps_data_->gps_lat,&gps_data_->gps_x,&gps_data_->gps_y);
+
+            mMutexMap.lock();
+            decition_gps = decitionGps00->getDecideFromGPS(*gps_data_, navigation_data, templidar,*lidar_per,trafficLight);  // my dicition=============================
+            mMutexMap.unlock();
+
+            if(mbUseExternMPC)
+            {
+                fanya::GPS_INS xgpsins;
+                xgpsins.speed_y = gps_data_->speed/3.6;
+                xgpsins.gps_lat = gps_data_->gps_lat;
+                xgpsins.gps_lng = gps_data_->gps_lng;
+                xgpsins.ins_heading = gps_data_->ins_heading_angle;
+                mmpcapi.SetGPS(xgpsins);
+                mmpcapi.SetDesiredspeed(decition_gps->speed/3.6);
+                double mpc_speed,mpc_decision,mpc_wheel;
+                if(mmpcapi.GetDecision(mpc_speed,mpc_decision,mpc_wheel) == 0)
+                {
+                    decition_gps->wheel_angle = mpc_wheel;
+                }
+            }
+            iv::modulecomm::ModuleSendMsg(mpaPlanTrace,(char *)decitionGps00->planTrace.data(),decitionGps00->planTrace.size()*sizeof(iv::Point2D));
+
+
+
+            //      decition_gps = decitionLine00->getDecideFromGPS(*gps_data_, navigation_data, obs_lidar_grid_);
+            //ODS("\n决策：%f\t%f\t\t", decition_gps->speed, decition_gps->wheel_angle);
+            //ODS("\n接收到的实时GPS数据:%f\t%f\t%f\n", gps_data_->gps_x, gps_data_->gps_y,gps_data_->ins_heading_angle);
+
+            //carcall->is_arrived = decitionGps00->is_arrivaled;
+            //double end = GetTickCount();
+            decision_period = start - last;
+            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n周期时长:%f\n" << decision_period << std::endl;
+            //                iv::decition::VehicleStateBasic vsb;
+            //                vsb.mbBocheEnable = ServiceCarStatus.bocheEnable;
+            //                vsb.mfLidarObs = ServiceCarStatus.mLidarObs;
+            //                vsb.mfRadarObs = ServiceCarStatus.mRadarObs;
+            //                vsb.mfObs = ServiceCarStatus.mRadarObs;
+            //                vsb.decision_period = decision_period;
+            //                vsb.mbBrainRunning = ServiceCarStatus.mbBrainCtring;
+            ////                mpaVechicleState->writemsg((char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
+
+            //                iv::modulecomm::ModuleSendMsg(mpaVechicleState,(char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
+
+            iv::brain::brainstate xbs;
+            xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
+            xbs.set_mbbrainrunning(true);  //apollo_fu debug ui 20200417
+            xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
+            xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
+            xbs.set_mfobs(ServiceCarStatus.mObs);
+            xbs.set_decision_period(decision_period);
+            ShareBrainstate(xbs);
+
+
+            //                decitionExecuter->executeDecition(decition_gps);
+            ShareDecition(decition_gps);
+
+            givlog->debug("acc is %f brake is %f wheel is %f",
+                          decition_gps->accelerator,decition_gps->brake,
+                          decition_gps->wheel_angle);
+            givlog->debug("period id %f",decision_period);
+
+            //	ODS("\n周期时长:%f\n", start - last);
+            //	ODS("\n决策时长:%f\n", end - start);
+            //	ODS("\n接收到的实时GPS车速:%f\n", gps_data_->speed);
+            //	ODS("\n接收到的实时GPS车y轴加速:%f\n", gps_data_->accel_y);
+            //	ODS("\n接收到的实时GPS车x轴加速:%f\n", gps_data_->accel_x);
+            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策加速度:%f\n" << decition_gps->accelerator << std::endl;
+            //		ODS("\n决策刹车:%f\n", decition_gps->brake);
+            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策方向:%f\n" << decition_gps->wheel_angle << std::endl;
+            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策速度:%f\n" << decition_gps->speed << std::endl;
+            last = start;
+            //decitionMaker->decideFromGPS(decition_gps, gps_data_, navigation_data);		//gps_data_为当前读到的gps位置信息  navigation_data为导航数据  decition_gps为根据前两者得出的决策速度与决策角度
+            //            }
+
+        }
+
+        if (handPark && decition_gps != NULL)
+        {
+            decition_gps->brake = 20;
+            decition_gps->accelerator = 0;
+            //			decitionExecuter->executeDecition(decition_gps);
+            ShareDecition(decition_gps);
+            //boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+
+#ifdef linux
+            usleep(2000000);
+#endif
+#ifdef WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+            //           Sleep(2000);
+#endif
+            decition_gps->brake = 0;
+            decition_gps->accelerator = 0;
+            //			decitionExecuter->executeDecition(decition_gps);
+            //			ServiceCanUtil.set_handbrake_on();
+            //boost::this_thread::sleep(boost::posix_time::milliseconds(handParkTime));
+            ShareDecition(decition_gps);
+
+#ifdef linux
+            usleep(handParkTime*1000);
+#endif
+#ifdef WIN32
+            Sleep(handParkTime);
+#endif
+            handPark = false;
+        }
+
+
+        else
+        {
+            //boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+
+#ifdef linux
+            usleep(1000);
+#endif
+#ifdef WIN32
+            Sleep(1);
+#endif
+            //			ODS("no gps data .sleep.\r\n");
+        }
+
+
+
+
+        //        if (decition_lidar || decition_radar || decition_camera || decition_gps || decition_lidar) {
+        //            //	decitionVoter->decideFromAll(decition_final, decition_lidar, decition_radar, decition_camera, decition_gps, decition_localmap);
+        //            //	decitionExecuter->executeDecition(decition_final);	//执行最终的决策
+        //            /*	decition_gps->brake = 0;
+        //            decition_gps->accelerator = 0;*/
+        //            look1 = decition_gps->accelerator;
+        //            look2 = decition_gps->brake;
+        //            look3 = decition_gps->wheel_angle;
+        //            look4 = decition_gps->speed;
+        //            look5 = decitionGps00->lidarDistance;
+        //            look6 = decitionGps00->myesrDistance;
+        //            look7 = decitionGps00->obsDistance;
+
+        //            decition_gps->grade=1;
+        //            decition_gps->mode=1;
+        //            decition_gps->speak=0;
+        //            decition_gps->handBrake=0;
+        //            decition_gps->bright=false;
+        //            decition_gps->engine=0;
+
+        //            if(ServiceCarStatus.bocheMode){
+        //                decition_gps->dangWei=2;
+        //            }else{
+        //                decition_gps->dangWei=1;
+        //            }
+
+
+        //            decition_gps->wheel_angle=max((float)-500.0,float(decition_gps->wheel_angle+ServiceCarStatus.mfEPSOff));
+        //            decition_gps->wheel_angle=min((float)500.0,decition_gps->wheel_angle);
+
+        //            //			decitionExecuter->executeDecition(decition_gps);
+        //            //           ShareDecition(decition_gps);
+        //        }
+
+        ServiceCarStatus.mfAcc = decition_gps->accelerator;
+        ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
+        ServiceCarStatus.mfBrake = decition_gps->brake;
+
+        iv::platform::PlatFormMsg toplat;
+        toplat.carState = ServiceCarStatus.carState;	// 0:停车	1:正常循迹	2:前往站点
+        toplat.istostation = ServiceCarStatus.istostation;
+        toplat.currentstation = ServiceCarStatus.currentstation;
+        toplat.clientto = ServiceCarStatus.clientto;
+        toplat.amilng = ServiceCarStatus.amilng;
+        toplat.amilat = ServiceCarStatus.amilat;
+        //       mpaToPlatform->writemsg((char*)&toplat,sizeof(iv::platform::PlatFormMsg));
+
+        iv::modulecomm::ModuleSendMsg(mpaToPlatform,(char*)&toplat,sizeof(iv::platform::PlatFormMsg));
+        //ODS("\ngetSensor时长:%f\n", end1 - start1);
+    }
+    std::cout << "\n\n\n\n\n\n\n\n\n\n\nbrain线程退出\n" << std::endl;
+}
+
+bool iv::decition::BrainDecition::loadMapFromFile(std::string fileName) {
+    std::ifstream fis(fileName);//获取文件
+    std::string line;
+    std::vector<std::string> des;
+
+    if (fis.is_open() == false)
+        return false;
+    try {
+        while (std::getline(fis, line)) {//开始一行一行的读数据
+            boost::split(des, line, boost::is_any_of("\t"));
+            if (des.size() != 10)
+                throw "error";
+            iv::GPSData data(new iv::GPS_INS);
+            data->index = atoi(des[0].c_str());
+            data->gps_lng = atof(des[1].c_str());
+            data->gps_lat = atof(des[2].c_str());
+            data->speed_mode = atoi(des[3].c_str());
+            data->mode2 = atoi(des[4].c_str());
+            data->ins_heading_angle = atof(des[5].c_str());
+            data->runMode = atoi(des[6].c_str());
+            data->roadMode = atoi(des[7].c_str());
+            data->roadSum = atoi(des[8].c_str());
+            data->roadOri = atoi(des[9].c_str());
+            //      gps2xy(data->gps_lat, data->gps_lng, &data->gps_x, &data->gps_y);
+            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+            //data->speed = 5;	//调试用  所有点速度都设为5km/h
+            //LOG2(data->gps_x, data->gps_y);
+            //ODS("x %.20lf   y %.20lf\n", data->gps_x, data->gps_y);
+
+            //	iv::decition::BLH2XYZ(*data);   //重新转下大地坐标
+
+            if(data->roadMode==4){
+                data->ins_heading_angle=data->ins_heading_angle+180;
+                if(data->ins_heading_angle>=360)
+                    data->ins_heading_angle=data->ins_heading_angle-360;
+            }
+
+            navigation_data.push_back(data);
+        }
+
+    }
+    catch (...) {
+        fis.close();
+        return false;
+    }
+
+    fis.close();
+    return true;
+}
+
+bool iv::decition::BrainDecition::loadMapFromFile2(std::string fileName) {
+    std::ifstream fis(fileName);//获取文件
+    std::string line;
+    std::vector<std::string> des;
+
+    if (fis.is_open() == false)
+        return false;
+    try {
+        while (std::getline(fis, line)) {//开始一行一行的读数据
+            boost::split(des, line, boost::is_any_of("\t"));
+            if (des.size() != 10)
+                throw "error";
+            iv::GPSData data(new iv::GPS_INS);
+            data->index = atoi(des[0].c_str());
+            data->gps_lng = atof(des[1].c_str());
+            data->gps_lat = atof(des[2].c_str());
+            data->speed_mode = atoi(des[3].c_str());
+            data->mode2 = atoi(des[4].c_str());
+            data->ins_heading_angle = atof(des[5].c_str());
+            data->runMode = atoi(des[6].c_str());
+            data->roadMode = atoi(des[7].c_str());
+            data->roadSum = atoi(des[8].c_str());
+            data->roadOri = atoi(des[9].c_str());
+            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+            navigation_data2.push_back(data);
+        }
+    }
+    catch (...) {
+        fis.close();
+        return false;
+    }
+
+    fis.close();
+    return true;
+}
+
+bool iv::decition::BrainDecition::loadMapFromFile3(std::string fileName) {
+    std::ifstream fis(fileName);//获取文件
+    std::string line;
+    std::vector<std::string> des;
+
+    if (fis.is_open() == false)
+        return false;
+    try {
+        while (std::getline(fis, line)) {//开始一行一行的读数据
+            boost::split(des, line, boost::is_any_of("\t"));
+            if (des.size() != 10)
+                throw "error";
+            iv::GPSData data(new iv::GPS_INS);
+            data->index = atoi(des[0].c_str());
+            data->gps_lng = atof(des[1].c_str());
+            data->gps_lat = atof(des[2].c_str());
+            data->speed_mode = atoi(des[3].c_str());
+            data->mode2 = atoi(des[4].c_str());
+            data->ins_heading_angle = atof(des[5].c_str());
+            data->runMode = atoi(des[6].c_str());
+            data->roadMode = atoi(des[7].c_str());
+            data->roadSum = atoi(des[8].c_str());
+            data->roadOri = atoi(des[9].c_str());
+            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+            navigation_data3.push_back(data);
+        }
+    }
+    catch (...) {
+        fis.close();
+        return false;
+    }
+
+    fis.close();
+    return true;
+}
+
+bool iv::decition::BrainDecition::loadMapFromFile4(std::string fileName) {
+    std::ifstream fis(fileName);//获取文件
+    std::string line;
+    std::vector<std::string> des;
+
+    if (fis.is_open() == false)
+        return false;
+    try {
+        while (std::getline(fis, line)) {//开始一行一行的读数据
+            boost::split(des, line, boost::is_any_of("\t"));
+            if (des.size() != 10)
+                throw "error";
+            iv::GPSData data(new iv::GPS_INS);
+            data->index = atoi(des[0].c_str());
+            data->gps_lng = atof(des[1].c_str());
+            data->gps_lat = atof(des[2].c_str());
+            data->speed_mode = atoi(des[3].c_str());
+            data->mode2 = atoi(des[4].c_str());
+            data->ins_heading_angle = atof(des[5].c_str());
+            data->runMode = atoi(des[6].c_str());
+            data->roadMode = atoi(des[7].c_str());
+            data->roadSum = atoi(des[8].c_str());
+            data->roadOri = atoi(des[9].c_str());
+            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+            navigation_data4.push_back(data);
+        }
+    }
+    catch (...) {
+        fis.close();
+        return false;
+    }
+
+    fis.close();
+    return true;
+}
+
+void iv::decition::BrainDecition::start() {
+    thread_run = new boost::thread(boost::bind(&iv::decition::BrainDecition::run, this));
+}
+
+void iv::decition::BrainDecition::stop() {
+    //    carcall->stop();
+    thread_run->interrupt();
+    thread_run->join();
+}
+
+void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapdatasize)
+{
+    //    std::cout<<"update map "<<std::endl;
+    int gpsunitsize = sizeof(iv::GPS_INS);
+    int nMapSize = mapdatasize/gpsunitsize;
+    //    std::cout<<"map size is "<<nMapSize<<std::endl;
+
+    if(nMapSize < 1)return;
+
+    bool bUpdate = false;
+    if(nMapSize != navigation_data.size())
+    {
+        bUpdate = true;
+    }
+    else
+    {
+        iv::GPS_INS * p = (iv::GPS_INS *)mapdata;
+        if((p->gps_lat == navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == navigation_data.at(0)->ins_heading_angle))
+        {
+            //           qDebug("same map");
+            bUpdate = false;
+        }
+        else
+        {
+            bUpdate = true;
+        }
+    }
+
+    if(bUpdate)
+    {
+        std::vector<fanya::MAP_DATA> xvectorMAP;
+        int i;
+        mMutexMap.lock();
+        navigation_data.clear();
+        for(i=0;i<nMapSize;i++)
+        {
+            iv::GPS_INS x;
+            memcpy(&x,mapdata + i*gpsunitsize,gpsunitsize);
+            iv::GPSData data(new iv::GPS_INS);
+            *data = x;
+            navigation_data.push_back(data);
+
+            fanya::MAP_DATA xmapdata;
+            xmapdata.gps_lat = x.gps_lat;
+            xmapdata.gps_lng = x.gps_lng;
+            xmapdata.ins_heading = x.ins_heading_angle;
+            xvectorMAP.push_back(xmapdata);
+        }
+
+        mmpcapi.SetMAP(xvectorMAP);
+
+        if(ServiceCarStatus.speed_control==true){
+        Compute00().getDesiredSpeed(navigation_data);}
+        mMutexMap.unlock();
+        //        mpasd->SaveState("map",mapdata,mapdatasize);
+    }
+    else
+    {
+        //       qDebug("not need update");
+    }
+}
+
+void iv::decition::BrainDecition::ShareDecition(iv::decition::Decition decition)
+{
+    iv::brain::decition xdecition;
+    xdecition.set_accelerator(decition->accelerator);
+    xdecition.set_brake(decition->brake);
+    xdecition.set_leftlamp(decition->leftlamp);
+    xdecition.set_rightlamp(decition->rightlamp);
+    xdecition.set_speed(decition->speed);
+    xdecition.set_wheelangle(decition->wheel_angle);
+    xdecition.set_wheelspeed(decition->angSpeed);
+    xdecition.set_torque(decition->torque);
+    xdecition.set_mode(decition->mode);
+    xdecition.set_gear(decition->dangWei);
+    xdecition.set_handbrake(decition->handBrake);
+    xdecition.set_grade(decition->grade);
+    xdecition.set_engine(decition->engine);
+    xdecition.set_brakelamp(decition->brakeLight);
+    xdecition.set_ttc(ServiceCarStatus.mfttc);
+    xdecition.set_air_enable(decition->air_enable);
+    xdecition.set_air_temp(decition->air_temp);
+    xdecition.set_air_mode(decition->air_mode);
+    xdecition.set_wind_level(decition->wind_level);
+    xdecition.set_roof_light(decition->roof_light);
+    xdecition.set_home_light(decition->home_light);
+    xdecition.set_air_worktime(decition->air_worktime);
+    xdecition.set_air_offtime(decition->air_offtime);
+    xdecition.set_air_on(decition->air_on);
+
+
+    std::cout<<"===================shareDecition========================"<<std::endl;
+         std::cout<<"  torque_st:"<<ServiceCarStatus.torque_st
+                 <<"  decideTorque:"<<decition->torque <<"  decideBrake:"<<decition->brake
+                <<"  decition mode:"<<decition->mode
+        <<std::endl;
+    std::cout<<"========================================="<<std::endl;
+
+    givlog->verbose("torque_st: %ld",ServiceCarStatus.torque_st);
+     givlog->verbose("decideTorque: %ld",decition->torque);
+     givlog->verbose("decideBrake: %ld", decition->brake);
+     givlog->debug("wheelAng: %f",decition->wheel_angle);
+
+   //  xdecition.set_wheelangle(100);
+    static qint64 oldtime;
+
+    if((QDateTime::currentMSecsSinceEpoch() - oldtime)>100)
+    {
+        givlog->warn("brain interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldtime);
+    }
+
+    givlog->verbose("decition time is %ld",QDateTime::currentMSecsSinceEpoch());
+    oldtime = QDateTime::currentMSecsSinceEpoch();
+
+
+    givlog->verbose("torque %f wheel %f dangwei %d ",decition->torque,
+                    decition->wheel_angle,decition->dangWei);
+    int nsize = xdecition.ByteSize();
+    char * str = new char[nsize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(xdecition.SerializeToArray(str,nsize))
+    {
+        if(ServiceCarStatus.mbRunPause == false)
+        {
+
+            iv::modulecomm::ModuleSendMsg(mpaDecition,str,nsize);
+        }
+    }
+    else
+    {
+        std::cout<<"iv::decition::BrainDecition::ShareDecition serialize error."<<std::endl;
+    }
+}
+
+void iv::decition::BrainDecition::ShareBrainstate(iv::brain::brainstate xbs)
+{
+    int nsize = xbs.ByteSize();
+    char * str = new char[nsize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(xbs.SerializeToArray(str,nsize))
+    {
+        iv::modulecomm::ModuleSendMsg(mpaVechicleState,str,nsize);
+    }
+    else
+    {
+        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
+    }
+}
+
+void iv::decition::BrainDecition::UpdateHMI(const char *pdata, const int ndatasize)
+{
+    if(ndatasize < sizeof(iv::hmi::HMIBasic))return;
+
+    iv::hmi::hmimsg xhmimsg;
+    if(!xhmimsg.ParseFromArray(pdata,ndatasize))
+    {
+        givlog->error("iv::decition::BrainDecition::UpdateHMI parse error");
+        return;
+    }
+
+    ServiceCarStatus.mbRunPause = xhmimsg.mbpause();
+
+    if(xhmimsg.mbbochemode()){
+         ServiceCarStatus.bocheMode = true;
+    }
+    ServiceCarStatus.busmode = xhmimsg.mbbusmode();
+
+    if( ServiceCarStatus.bocheMode ){
+        int a=0;
+    }
+
+    bool bRun;
+    bRun = !ServiceCarStatus.mbRunPause;
+
+    //    mpasd->SaveState("runstate",(char *)&bRun,sizeof(bool));
+
+}
+
+void iv::decition::BrainDecition::UpdatePlatform(const char *pdata, const int ndatasize)
+{
+    if(ndatasize < sizeof(iv::platform::PlatFormMsg))return;
+
+    iv::platform::PlatFormMsg x;
+    memcpy(&x,pdata,sizeof(iv::platform::PlatFormMsg));
+    ServiceCarStatus.carState = x.carState;	// 0:停车	1:正常循迹	2:前往站点
+    std::cout<<"car state "<<ServiceCarStatus.carState<<std::endl;
+    ServiceCarStatus.istostation = x.istostation;
+    ServiceCarStatus.currentstation = x.currentstation;
+    ServiceCarStatus.clientto = x.clientto;
+    ServiceCarStatus.amilng = x.amilng;
+    ServiceCarStatus.amilat = x.amilat;
+
+}
+
+void iv::decition::BrainDecition::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    iv::chassis xchassis;
+    static int ncount = 0;
+    if(!xchassis.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
+        return;
+    }
+
+    ServiceCarStatus.epb = xchassis.epbfault();
+    ServiceCarStatus.grade = xchassis.shift();
+    ServiceCarStatus.driverMode = xchassis.drivemode();
+    if(xchassis.has_epsmode()){
+        ServiceCarStatus.epsMode = xchassis.epsmode();
+        ServiceCarStatus.receiveEps=true;
+        if(ServiceCarStatus.epsMode == 0)
+        {
+            ServiceCarStatus.mbRunPause = true;
+        }
+    }
+    if(xchassis.has_torque())
+    {
+         ServiceCarStatus.torque_st = xchassis.torque();
+        if(ServiceCarStatus.msysparam.mvehtype=="bus"){
+            ServiceCarStatus.torque_st = xchassis.torque()*0.4;
+        }
+        std::cout<<"******* ServiceCarStatus.torque_st:"<< ServiceCarStatus.torque_st<<std::endl;
+        givlog->warn(" ServiceCarStatus.torque_st: is %f",ServiceCarStatus.torque_st);
+
+    }
+    //    if(xchassis.epsmode() == 1)
+    //    {
+    //        ncount++;
+    ////        if(ncount > 10)ServiceCarStatus.mbRunPause = true;
+    //    }
+    //    else
+    //    {
+    //        ncount = 0;
+    //    }
+}
+
+void iv::decition::BrainDecition::UpdateGroupInfo(const char *pdata, const int ndatasize){
+
+    iv::radio::radio_send group_message;
+    if(false == group_message.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Listencansend Parse fail."<<std::endl;
+        return;
+    }
+    ServiceCarStatus.group_server_status = group_message.server_status();
+    ServiceCarStatus.group_comm_speed= (float)group_message.car_control_speed();
+    ServiceCarStatus.group_state= group_message.cmd_mode();
+
+    qDebug("updateGroupInfo %ld",QDateTime::currentMSecsSinceEpoch());
+    if(ServiceCarStatus.group_state!=2){
+        ServiceCarStatus.group_comm_speed=0;
+    }
+    qDebug("serverSt: %d, speed: %f, state: %d", ServiceCarStatus.group_server_status, ServiceCarStatus.group_comm_speed, ServiceCarStatus.group_state);
+
+
+}
+
+void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatasize){
+
+    iv::vbox::vbox group_message;
+    if(false == group_message.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Listencansend Parse fail."<<std::endl;
+        return;
+    }
+
+//  解析box信息
+//    ServiceCarStatus.group_server_status = group_message.server_status();
+//    ServiceCarStatus.group_comm_speed= (float)group_message.car_control_speed();
+//    ServiceCarStatus.group_state= group_message.cmd_mode();
+
+
+    trafficLight.leftColor=group_message.st_left();
+    trafficLight.rightColor=group_message.st_right();
+    trafficLight.straightColor=group_message.st_straight();
+    trafficLight.uturnColor=group_message.st_turn();
+    trafficLight.leftTime=group_message.time_left();
+    trafficLight.rightTime=group_message.time_right();
+    trafficLight.straightTime=group_message.time_straight();
+    trafficLight.uturnTime=group_message.time_turn();
+
+
+
+}
+
+void iv::decition::BrainDecition::UpdateSate(){
+     decitionGps00->isFirstRun=true;
+}

src/decition/decition_brain_sf/decition/brain.h

@@ -1,168 +1,168 @@
-#pragma once
-/*
-* 中央决策大脑
-*/
-
-#ifndef _IV_DECITION_BRAIN_
-#define _IV_DECITION_BRAIN_
-
-#include <common_sf/boost.h>
-#include <common_sf/gps_type.h>
-//#include <control/controller.h>
-//#include <control/can.h>
-#include <perception/eyes.h>
-#include <decition/decition_maker.h>
-//#include <decition/decition_executer.h>
-#include <decition/decition_voter.h>
-#include <decition/decide_gps_00.h>
-#include <decition/decide_line_00.h>
-#include <decition/adc_tools/compute_00.h>
-#include <perception/sensor.h>
-#include <perception/sensor_camera.h>
-#include <perception/sensor_gps.h>
-#include<perception/sensor_lidar.h>
-#include<perception/sensor_radar.h>
-//#include <server/carcalling.h>
-//#include "adcivstatedb.h"
-
-//#include "decition/vehiclestate_type.h"
-#include "common_sf/hmi_type.h"
-#include "common_sf/platform_type.h"
-#include"common_sf/gps_type.h"
-
-#include <QMutex>
-#include <QTime>
-#include <memory>
-
-#include "brainstate.pb.h"
-#include "decition.pb.h"
-#include "mapreq.pb.h"
-#include "chassis.pb.h"
-#include "hmi.pb.h"
-#include "radio_send.pb.h"
-#include "ivlog.h"
-#include "formation_map.pb.h"
-#include "vbox.pb.h"
-
-#include "fanyaapi.h"
-
-namespace iv {
-	namespace decition {
-		class BrainDecition
-		{
-		public:
-			BrainDecition();
-			~BrainDecition();
-
-
-            void inialize();/* 初始化*/
-
-
-            bool loadMapFromFile(std::string fileName);/* 加载地图*/
-			bool loadMapFromFile2(std::string fileName);
-			bool loadMapFromFile3(std::string fileName);
-			bool loadMapFromFile4(std::string fileName);
-
-
-            void start();/* 启动大脑*/
-			void stop();	//关闭
-
-			std::vector<iv::GPSData> navigation_data;	//导航数据，GPS结构体数组
-			std::vector<iv::GPSData> navigation_data2;	//导航数据，GPS结构体数组2
-			std::vector<iv::GPSData> navigation_data3;	//导航数据，GPS结构体数组3
-			std::vector<iv::GPSData> navigation_data4;	//导航数据，GPS结构体数组4
-			std::vector<iv::ObsRadar> radar_data;
-			std::vector<iv::ObsRadar> lidar_data;
-            std::vector<iv::ObsRadar> camera_data;
-		/*	std::vector<std::vector<iv::GPSData>> mapsL;
-			std::vector<std::vector<iv::GPSData>> mapsR;*/
-
-            double decision_period = 0.0;
-            double look1 = 0.0;
-            double look2 = 0.0;
-            double look3 = 0.0;
-            double look4 = 0.0;
-            double look5 = 0.0;
-            double look6 = 0.0;
-            double look7 = 0.0;
-
-            void UpdateMap(const char * mapdata,const int mapdatasize);
-		private:
-		//	iv::perception::Eyes* eyes;							//眼睛
- //           iv::carcalling::carcalling* carcall;
-			iv::perception::Eyes* eyes;
-			iv::decition::DecitionMaker* decitionMaker;			//决策器
-			iv::decition::DecitionVoter* decitionVoter;			//决策仲裁 判断器
-//			iv::decition::DecitionExecuter* decitionExecuter;	//决策执行器
-
-            iv::decition::DecideGps00* decitionGps00;			//决策器
-              iv::decition::DecideLine00* decitionLine00;
-
-//			boost::shared_ptr<iv::control::Controller> controller;	//实际车辆控制器
-
-			boost::thread* thread_run;
-			
-			iv::ObsLiDAR obs_lidar_;		//激光雷达障碍物
-			iv::ObsRadar obs_radar_;		//毫米波雷达障碍物
-			//iv::ObsCamera obs_camera_;		//摄像头障碍物
-			iv::CameraInfoPtr obs_camera_;
-//			iv::GPSData gps_data_;			//gps 惯导数据
-			iv::LidarGridPtr obs_lidar_grid_;//激光雷达网格化
-            iv::LidarGridPtr old_obs_lidar_grid_;//激光雷达网格化
-            iv::TrafficLight trafficLight;
-            iv::Obs_grid* obs_fusion_grid_;     //fusion网格化
-
-
-
-            int lastMode;
-            bool lastPause;
-
-			void run();	//实际执行逻辑
-
-            void * mpamapreq;
-            void * mpa;
-            void * mpvbox;
-            QMutex mMutexMap;
-
-            void * mpaDecition;
-            void * mpaVechicleState;
-            void * mpaToPlatform;
-            void * mpaPlanTrace;
-
-
-            void ShareDecition(iv::decition::Decition decition);
-            void ShareBrainstate(iv::brain::brainstate xbs);
-
-        private:
-            void * mpaHMI;
-            void * mpaPlatform;
-            void *mpaGroup;
-            void * mpmapChangeSig;
-            std::string mstrmemmap_index;
-
-            int mnOldTime;
-            QTime mTime;
-            double mfSpeedLast;
-            double mfVehSpeedLast;
-
-
-        public:
-            void UpdateHMI(const char * pdata,const int ndatasize);
-            void UpdatePlatform(const char * pdata,const int ndatasize);
-            void UpdateGroupInfo(const char * pdata,const int ndatasize);
-            void UpdateSate();
-            void UpdateVbox(const char * pdata,const int ndatasize);
-
-        private:
-//            Adcivstatedb * mpasd;
-            void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-
-            fanyaapi mmpcapi;
-
-            bool mbUseExternMPC = false;
-        };
-
-    }
-}
-
-#endif // !_IV_DECITION_BRAIN_
+#pragma once
+/*
+* 中央决策大脑
+*/
+
+#ifndef _IV_DECITION_BRAIN_
+#define _IV_DECITION_BRAIN_
+
+#include <common_sf/boost.h>
+#include <common_sf/gps_type.h>
+//#include <control/controller.h>
+//#include <control/can.h>
+#include <perception/eyes.h>
+#include <decition/decition_maker.h>
+//#include <decition/decition_executer.h>
+#include <decition/decition_voter.h>
+#include <decition/decide_gps_00.h>
+#include <decition/decide_line_00.h>
+#include <decition/adc_tools/compute_00.h>
+#include <perception/sensor.h>
+#include <perception/sensor_camera.h>
+#include <perception/sensor_gps.h>
+#include<perception/sensor_lidar.h>
+#include<perception/sensor_radar.h>
+//#include <server/carcalling.h>
+//#include "adcivstatedb.h"
+
+//#include "decition/vehiclestate_type.h"
+#include "common_sf/hmi_type.h"
+#include "common_sf/platform_type.h"
+#include"common_sf/gps_type.h"
+
+#include <QMutex>
+#include <QTime>
+#include <memory>
+
+#include "brainstate.pb.h"
+#include "decition.pb.h"
+#include "mapreq.pb.h"
+#include "chassis.pb.h"
+#include "hmi.pb.h"
+#include "radio_send.pb.h"
+#include "ivlog.h"
+#include "formation_map.pb.h"
+#include "vbox.pb.h"
+
+#include "fanyaapi.h"
+
+namespace iv {
+	namespace decition {
+		class BrainDecition
+		{
+		public:
+			BrainDecition();
+			~BrainDecition();
+
+
+            void inialize();/* 初始化*/
+
+
+            bool loadMapFromFile(std::string fileName);/* 加载地图*/
+			bool loadMapFromFile2(std::string fileName);
+			bool loadMapFromFile3(std::string fileName);
+			bool loadMapFromFile4(std::string fileName);
+
+
+            void start();/* 启动大脑*/
+			void stop();	//关闭
+
+			std::vector<iv::GPSData> navigation_data;	//导航数据，GPS结构体数组
+			std::vector<iv::GPSData> navigation_data2;	//导航数据，GPS结构体数组2
+			std::vector<iv::GPSData> navigation_data3;	//导航数据，GPS结构体数组3
+			std::vector<iv::GPSData> navigation_data4;	//导航数据，GPS结构体数组4
+			std::vector<iv::ObsRadar> radar_data;
+			std::vector<iv::ObsRadar> lidar_data;
+            std::vector<iv::ObsRadar> camera_data;
+		/*	std::vector<std::vector<iv::GPSData>> mapsL;
+			std::vector<std::vector<iv::GPSData>> mapsR;*/
+
+            double decision_period = 0.0;
+            double look1 = 0.0;
+            double look2 = 0.0;
+            double look3 = 0.0;
+            double look4 = 0.0;
+            double look5 = 0.0;
+            double look6 = 0.0;
+            double look7 = 0.0;
+
+            void UpdateMap(const char * mapdata,const int mapdatasize);
+		private:
+		//	iv::perception::Eyes* eyes;							//眼睛
+ //           iv::carcalling::carcalling* carcall;
+			iv::perception::Eyes* eyes;
+			iv::decition::DecitionMaker* decitionMaker;			//决策器
+			iv::decition::DecitionVoter* decitionVoter;			//决策仲裁 判断器
+//			iv::decition::DecitionExecuter* decitionExecuter;	//决策执行器
+
+            iv::decition::DecideGps00* decitionGps00;			//决策器
+              iv::decition::DecideLine00* decitionLine00;
+
+//			boost::shared_ptr<iv::control::Controller> controller;	//实际车辆控制器
+
+			boost::thread* thread_run;
+			
+			iv::ObsLiDAR obs_lidar_;		//激光雷达障碍物
+			iv::ObsRadar obs_radar_;		//毫米波雷达障碍物
+			//iv::ObsCamera obs_camera_;		//摄像头障碍物
+			iv::CameraInfoPtr obs_camera_;
+//			iv::GPSData gps_data_;			//gps 惯导数据
+			iv::LidarGridPtr obs_lidar_grid_;//激光雷达网格化
+            iv::LidarGridPtr old_obs_lidar_grid_;//激光雷达网格化
+            iv::TrafficLight trafficLight;
+            iv::Obs_grid* obs_fusion_grid_;     //fusion网格化
+
+
+
+            int lastMode;
+            bool lastPause;
+
+			void run();	//实际执行逻辑
+
+            void * mpamapreq;
+            void * mpa;
+            void * mpvbox;
+            QMutex mMutexMap;
+
+            void * mpaDecition;
+            void * mpaVechicleState;
+            void * mpaToPlatform;
+            void * mpaPlanTrace;
+
+
+            void ShareDecition(iv::decition::Decition decition);
+            void ShareBrainstate(iv::brain::brainstate xbs);
+
+        private:
+            void * mpaHMI;
+            void * mpaPlatform;
+            void *mpaGroup;
+            void * mpmapChangeSig;
+            std::string mstrmemmap_index;
+
+            int mnOldTime;
+            QTime mTime;
+            double mfSpeedLast;
+            double mfVehSpeedLast;
+
+
+        public:
+            void UpdateHMI(const char * pdata,const int ndatasize);
+            void UpdatePlatform(const char * pdata,const int ndatasize);
+            void UpdateGroupInfo(const char * pdata,const int ndatasize);
+            void UpdateSate();
+            void UpdateVbox(const char * pdata,const int ndatasize);
+
+        private:
+//            Adcivstatedb * mpasd;
+            void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+            fanyaapi mmpcapi;
+
+            bool mbUseExternMPC = false;
+        };
+
+    }
+}
+
+#endif // !_IV_DECITION_BRAIN_

src/decition/decition_brain_sf/decition/compute_00.cpp

@@ -1,1843 +1,1843 @@
-#include <decition/compute_00.h>
-#include <decition/decide_gps_00.h>
-#include <decition/gps_distance.h>
-#include <decition/decition_type.h>
-#include <decition/transfer.h>
-#include <common/constants.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-//#include <control/radar_exam.h>
-#include <control/can.h>
-using namespace std;
-
-#define PI (3.1415926535897932384626433832795)
-
-iv::decition::Compute00::Compute00() {
-
-}
-iv::decition::Compute00::~Compute00() {
-
-}
-
-
-double iv::decition::Compute00::angleLimit = 700;
-double iv::decition::Compute00::lastEA = 0;
-double iv::decition::Compute00::lastEP = 0;
-double iv::decition::Compute00::decision_Angle = 0;
-double iv::decition::Compute00::lastAng = 0;
-int iv::decition::Compute00::lastEsrID = -1;
-int  iv::decition::Compute00::lastEsrCount = 0;
-int iv::decition::Compute00::lastEsrIDAvoid = -1;
-int  iv::decition::Compute00::lastEsrCountAvoid = 0;
-
-iv::GPS_INS  iv::decition::Compute00::nearTpoint;
-iv::GPS_INS  iv::decition::Compute00::farTpoint;
-double  iv::decition::Compute00::bocheAngle;
-
-
-
-iv::GPS_INS  iv::decition::Compute00::dTpoint0;
-iv::GPS_INS  iv::decition::Compute00::dTpoint1;
-iv::GPS_INS  iv::decition::Compute00::dTpoint2;
-iv::GPS_INS  iv::decition::Compute00::dTpoint3;
-double  iv::decition::Compute00::dBocheAngle;
-
-
-std::vector<int> lastEsrIdVector;
-std::vector<int> lastEsrCountVector;
-
-int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
-{
-    int index = -1;
-    //	DecideGps00().minDis = iv::MaxValue;
-    DecideGps00().minDis = 20;
-    DecideGps00().maxAngle = iv::MaxValue;
-
-    int startIndex = max((lastIndex - 500), 0);     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = min((gpsMap.size() - 1), (size_t)(lastIndex + 2000 + 100 * (gpsMissCount + 1)));
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < DecideGps00().minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
-                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
-                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
-                )
-        {
-            index = i;
-            DecideGps00().minDis = tmpdis;
-            DecideGps00().maxAngle = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
-            DecideGps00().maxAngle = min(DecideGps00().maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
-        }
-    }
-    /*if (index == -1) {
-        index = 0;
-    }*/
-    return index;
-}
-
-
-//首次找点
-
-int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
-{
-    int index = -1;
-    //	DecideGps00().minDis = iv::MaxValue;
-    DecideGps00().minDis = 40;
-    DecideGps00().maxAngle = iv::MaxValue;
-
-    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = gpsMap.size() - 1;
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < DecideGps00().minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
-                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
-                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
-                )
-        {
-            index = i;
-            DecideGps00().minDis = tmpdis;
-            DecideGps00().maxAngle = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
-            DecideGps00().maxAngle = min(DecideGps00().maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
-        }
-    }
-    /*if (index == -1) {
-        index = 0;
-    }*/
-    return index;
-}
-
-
-
-
-
-double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
-{
-    double sum_x = 0;
-    double sum_y = 0;
-
-    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
-    {
-        sum_x += farTrace[i].x;
-        sum_y += abs(farTrace[i].y);
-    }
-    double average_y = sum_y / min(5, (int)farTrace.size());
-    double average_x = sum_x / min(5, (int)farTrace.size());
-
-
-    return atan(average_x / average_y) / PI * 180;
-}
-
-
-
-double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
-{
-    double sum_x = 0;
-    double sum_y = 0;
-
-    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
-    {
-        sum_x += farTrace[i].x;
-        sum_y += abs(farTrace[i].y);
-    }
-    double average_y = sum_y / min(5, (int)farTrace.size());
-    double average_x = sum_x / min(5, (int)farTrace.size());
-
-
-    return atan(average_x + avoidX / average_y) / PI * 180;
-}
-
-
-
-
-
-
-double iv::decition::Compute00::getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-
-    //   double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1;  // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-
-    if(transferPieriod&& !transferPieriod2){
-        DEang = 200;
-        DEPos = 150;
-    }
-
-    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-    double PreviewDistance;//预瞄距离
-    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-    if(changeRoad ||transferPieriod){
-        PreviewDistance=PreviewDistance+avoidX;
-    }
-    if(realSpeed <15){
-        PreviewDistance = max(4.0, realSpeed *0.4) ;
-    }
-
-    if (gpsTrace[0].v1 == 1)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
-    {
-        KEang = 18; KEPos = 50; PreviewDistance = 3;
-    }
-    else if (gpsTrace[0].v1 == 7)
-    {
-        KEang = 20; KEPos = 50; PreviewDistance = 4;
-    }
-
-
-    if (realSpeed > 40)	KEang = 10; KEPos = 8;
-    if (realSpeed > 50) KEang = 5;
-
-    double sumdis = 0;
-    int gpsIndex = 0;
-    std::vector<Point2D> farTrace;
-
-
-
-
-
-
-    for (int i = 1; i < gpsTrace.size() - 1; i++)
-    {
-        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
-        if (sumdis > PreviewDistance)
-        {
-            gpsIndex = i;
-            break;
-        }
-    }
-
-
-
-
-    EPos = gpsTrace[gpsIndex].x;
-
-    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
-        farTrace.push_back(gpsTrace[gpsIndex]);
-    }
-    if (farTrace.size() == 0) {
-        EAng = 0;
-    }
-    else {
-        EAng = getAveDef(farTrace);
-    }
-
-    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-    return ang;
-}
-
-
-
-
-int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
-{
-    int index = 1;
-    double sumdis = 0;
-    while (index < gpsTrace.size() && sumdis < realSpeed)
-        sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);
-
-    if (index == gpsTrace.size())
-        return index - 1;
-
-    if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
-        index--;
-    return index;
-}
-
-iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    for (int j = 0; j < gpsTrace.size(); j++)
-    {
-        double sumx1 = 0, sumy1 = 0, count1 = 0;
-        double sumx2 = 0, sumy2 = 0, count2 = 0;
-        for (int k = max(0, j - 4); k <= j; k++)
-        {
-            count1 = count1 + 1;
-            sumx1 += gpsTrace[k].x;
-            sumy1 += gpsTrace[k].y;
-        }
-        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
-        {
-            count2 = count2 + 1;
-            sumx2 += gpsTrace[k].x;
-            sumy2 += gpsTrace[k].y;
-        }
-        sumx1 /= count1; sumy1 /= count1;
-        sumx2 /= count2; sumy2 /= count2;
-
-        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
-
-        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
-        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
-
-        Point2D ptLeft(carFrontx + Veh_Width / 2 * cos(anglevalue + PI / 2),
-                       carFronty + Veh_Width / 2 * sin(anglevalue + PI / 2));
-
-        Point2D ptRight(carFrontx + Veh_Width / 2 * cos(anglevalue - PI / 2),
-                        carFronty + Veh_Width / 2 * sin(anglevalue - PI / 2));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y>2.5 )
-        {
-            int count = 0;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-                int dy = (pty + gridwide*(double)centery)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-                int dy = (pty + gridwide*(double)centery)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                isRemove = true;
-                //		DecideGps00().lidarDistance = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistance = obsPoint.y;
-    return obsPoint;
-}
-
-//1220
-iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    for (int j = 0; j < gpsTrace.size(); j++)
-    {
-        double sumx1 = 0, sumy1 = 0, count1 = 0;
-        double sumx2 = 0, sumy2 = 0, count2 = 0;
-        for (int k = max(0, j - 4); k <= j; k++)
-        {
-            count1 = count1 + 1;
-            sumx1 += gpsTrace[k].x;
-            sumy1 += gpsTrace[k].y;
-        }
-        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
-        {
-            count2 = count2 + 1;
-            sumx2 += gpsTrace[k].x;
-            sumy2 += gpsTrace[k].y;
-        }
-        sumx1 /= count1; sumy1 /= count1;
-        sumx2 /= count2; sumy2 /= count2;
-
-        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
-
-        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
-        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
-
-        Point2D ptLeft(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue + PI / 2),
-                       carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue + PI / 2));
-
-        Point2D ptRight(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue - PI / 2),
-                        carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue - PI / 2));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y<-1.0 )
-        {
-            int count = 0;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-
-                dx=grx-dx;//1227
-
-                int dy = (pty + gridwide*(double)centery)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-                int dx = (ptx + gridwide*(double)centerx)/gridwide;
-
-                dx=grx-dx;//1227
-                int dy = (pty + gridwide*(double)centery)/gridwide;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                isRemove = true;
-                //		DecideGps00().lidarDistance = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistance = obsPoint.y;
-    return obsPoint;
-}
-
-
-iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
-
-    iv::Point2D obsPoint(-1, -1);
-    vector<Point2D> gpsTraceLeft;
-    vector<Point2D> gpsTraceRight;
-    for (int j = 0; j < gpsTrace.size(); j++)
-    {
-        double sumx1 = 0, sumy1 = 0, count1 = 0;
-        double sumx2 = 0, sumy2 = 0, count2 = 0;
-        for (int k = max(0, j - 4); k <= j; k++)
-        {
-            count1 = count1 + 1;
-            sumx1 += gpsTrace[k].x;
-            sumy1 += gpsTrace[k].y;
-        }
-        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
-        {
-            count2 = count2 + 1;
-            sumx2 += gpsTrace[k].x;
-            sumy2 += gpsTrace[k].y;
-        }
-        sumx1 /= count1; sumy1 /= count1;
-        sumx2 /= count2; sumy2 /= count2;
-
-        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
-
-        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
-        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
-
-        //1127 fanwei xiuzheng
-        float buchang=0;
-        Point2D ptLeft(carFrontx + (Veh_Width+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
-                       carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
-
-        Point2D ptRight(carFrontx +  (Veh_Width+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
-                        carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
-
-        gpsTraceLeft.push_back(ptLeft);
-        gpsTraceRight.push_back(ptRight);
-
-    }
-
-
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
-        {
-            int count = 0;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
-                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            j++;
-
-            for (double length = 0; length <= Veh_Width; length += 0.4)
-            {
-                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
-                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
-
-                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
-                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
-
-                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
-                {
-                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
-                    {
-                        count++; obsPoint.x = ptx; obsPoint.y = pty;
-                    }
-                }
-            }
-
-            if (count >= 2)
-            {
-                obsPoint.x = gpsTrace[j].x;
-                obsPoint.y = gpsTrace[j].y;
-
-                isRemove = true;
-                DecideGps00().lidarDistanceAvoid = obsPoint.y;
-                return obsPoint;
-            }
-        }
-    }
-    //	DecideGps00().lidarDistanceAvoid = obsPoint.y;
-
-    return obsPoint;
-}
-
-
-
-
-//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
-//	bool isRemove = false;
-//
-//	for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-//	{
-//
-//		for (int i = 0; i < esrRadars.size(); i++)
-//			if ((esrRadars[i].nomal_y) != 0)
-//			{
-//				double xxx = esrRadars[i].nomal_x + Esr_Offset;
-//				double yyy = esrRadars[i].nomal_y;
-//
-//				if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-//				{
-//
-//					if (lastEsrID == (esrRadars[i]).esr_ID)
-//					{
-//						lastEsrCount++;
-//					}
-//					else
-//					{
-//						lastEsrCount = 0;
-//					}
-//
-//					if (lastEsrCount >= 3)
-//					{
-//						return i;
-//					}
-//
-//					lastEsrID = (esrRadars[i]).esr_ID;
-//				}
-//			}
-//	}
-//	return -1;
-//}
-
-
-
-
-int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint) {
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        for (int i = 0; i < 64; i++)
-            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
-            {
-                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-                double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
-
-                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
-                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
-
-//优化
-//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*Veh_Width / 2.0+DecideGps00().xiuzhengCs)){
-//                    *esrPathpoint = j;
-//                    return i;
-//                }
-
-
-                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
-                {
-
-                    if (lastEsrID == i)
-                    {
-                        lastEsrCount++;
-                    }
-                    else
-                    {
-                        lastEsrCount = 0;
-                    }
-
-                    if(yyy>50 ){
-                        if (lastEsrCount >=200)
-                        {
-                            return i;
-                        }
-                    }
-                    else if (lastEsrCount >= 1)
-                    {
-                        return i;
-                    }
-
-                    lastEsrID = i;
-                }
-            }
-    }
-    return -1;
-}
-
-//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
-//    bool isRemove = false;
-
-//    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-//    {
-
-//        for (int i = 0; i < 64; i++)
-//            if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
-//            {
-//                double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
-//                double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;
-
-//                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
-//                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
-
-
-//                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-//                {
-
-//                    if (lastEsrID == i)
-//                    {
-//                        lastEsrCount++;
-//                    }
-//                    else
-//                    {
-//                        lastEsrCount = 0;
-//                    }
-
-//                    if(yyy>50 ){
-//                        if (lastEsrCount >=200)
-//                        {
-//                            return i;
-//                        }
-//                    }
-//                    else if (lastEsrCount >= 3)
-//                    {
-//                        return i;
-//                    }
-
-//                    lastEsrID = i;
-//                }
-//            }
-//    }
-//    return -1;
-//}
-
-
-
-int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
-    bool isRemove = false;
-
-    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
-    {
-
-        for (int i = 0; i < 64; i++)
-            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
-            {
-                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-                double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
-
-                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
-                {
-
-                    if (lastEsrIDAvoid == i)
-                    {
-                        lastEsrCountAvoid++;
-                    }
-                    else
-                    {
-                        lastEsrCountAvoid = 0;
-                    }
-
-                    if (lastEsrCountAvoid >= 6)
-                    {
-                        return i;
-                    }
-
-                    lastEsrIDAvoid = i;
-                }
-            }
-    }
-    return -1;
-}
-
-
-
-
-
-
-
-//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
-//	double obsSpeed = 0 - realSecSpeed;
-//	double minDis = iv::MaxValue;
-//	for (int i = 0; i < esrRadars.size(); i++)
-//		if ((esrRadars[i].nomal_y) != 0)
-//		{
-//			double xxx = esrRadars[i].nomal_x + Esr_Offset;
-//			double yyy = esrRadars[i].nomal_y;
-//
-//			if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-//			{
-//				double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-//				if (tmpDis < minDis)
-//				{
-//					minDis = tmpDis;
-//					obsSpeed = esrRadars[i].speed_y;
-//				}
-//			}
-//		}
-//
-//	return obsSpeed;
-//
-//
-//}
-
-
-
-
-double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
-    double obsSpeed = 0 - realSecSpeed;
-    double minDis = iv::MaxValue;
-    for (int i = 0; i < 64; i++)
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
-        {
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
-
-            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-            {
-                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-                if (tmpDis < minDis)
-                {
-                    minDis = tmpDis;
-                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
-                }
-            }
-        }
-
-    return obsSpeed;
-}
-
-
-
-
-double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-
-    double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-
-    if (gpsTrace[0].v1 == 1)
-    {
-        KEang = 10; KEPos = 8;
-        if (realSpeed > 60) KEang = 5;
-    }
-    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
-    {
-        KEang = 14; KEPos = 10;
-    }
-    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
-    {
-        KEang = 18; KEPos = 50; PreviewDistance = 3;
-    }
-    else if (gpsTrace[0].v1 == 7)
-    {
-        KEang = 20; KEPos = 50; PreviewDistance = 4;
-    }
-
-
-    double sumdis = 0;
-    int gpsIndex = 0;
-    std::vector<Point2D> farTrace;
-
-
-
-
-
-
-    for (int i = 1; i < gpsTrace.size() - 1; i++)
-    {
-        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
-        if (sumdis > PreviewDistance)
-        {
-            gpsIndex = i;
-            break;
-        }
-    }
-
-    if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
-    {
-        gpsIndex = DecideGps00().gpsLineParkIndex;
-    }
-
-
-
-    EPos = gpsTrace[gpsIndex].x + avoidX;
-
-    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
-        farTrace.push_back(gpsTrace[gpsIndex]);
-    }
-    if (farTrace.size() == 0) {
-        EAng = 0;
-    }
-    else {
-        EAng = getAvoidAveDef(farTrace, avoidX);
-    }
-
-    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-
-    return ang;
-}
-
-
-std::vector<iv::GPSData>   iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {
-
-    vector<vector<iv::GPSData>> maps;
-    vector<iv::GPSData> gpsMapLineBeside;
-    int sizeN = gpsMapLine.size();
-    for (int i = 1; i < sizeN; i++)
-    {
-        iv::GPSData gpsData(new GPS_INS);
-        double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
-        double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
-        double lng = ServiceCarStatus.location->ins_heading_angle;
-
-
-        double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
-        double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
-        double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
-        double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
-
-        //	memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));
-
-        gpsData->speed_mode = gpsMapLine[i]->speed_mode;
-        gpsData->gps_x = x0 + k1 * avoidX;
-        gpsData->gps_y = y0 + k2 * avoidX;
-        gpsMapLineBeside.push_back(gpsData);
-
-    }
-    return gpsMapLineBeside;
-
-}
-
-
-
-//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {
-
-//    double ang = 0;
-//    double EPos = 0, EAng = 0;
-
-// //   double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
-//       double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;
-
-// //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-//    double PreviewDistance;//预瞄距离
-//    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-
-
-
-
-
-////    if (realSpeed > 40)	KEang = 10; KEpos = 8;
-////        if (realSpeed > 50) KEang = 5;
-
-
-
-
-
-//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
-//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
-//double a = ServiceCarStatus.Lane.curvature;
-//double b = ServiceCarStatus.Lane.heading;
-//double c = (c1+c2)*0.5;
-//double x= PreviewDistance;
-//double y;
-
-
-//y=a*x*x+b*x+c;
-
-//   // EPos = y;
-//EPos=c;
-
-
-//  //  EAng=atan(2*a*x+b) / PI * 180;
-//    EAng=ServiceCarStatus.Lane.yaw;
-//        ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
-
-//        lastEA = EAng;
-//        lastEP = EPos;
-
-
-//           std::cout << "\nEPos:%f\n" << EPos << std::endl;
-//            std::cout << "\nEAng:%f\n" << EAng << std::endl;
-
-
-//        if (ang > angleLimit) {
-//            ang = angleLimit;
-//        }
-//        else if (ang < -angleLimit) {
-//            ang = -angleLimit;
-//        }
-//        if (lastAng != iv::MaxValue) {
-//            ang = 0.2 * lastAng + 0.8 * ang;
-//            //ODS("lastAng：%d\n", lastAng);
-//        }
-//        lastAng = ang;
-//        return ang;
-//    }
-
-
-
-double IEPos = 0, IEang = 0;
-
-
-double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
-
-    double ang = 0;
-    double EPos = 0, EAng = 0;
-    double Curve=0;
-    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
-    double KCurve=120;
-    double KIEPos = 0, KIEang = 0;
-    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
-
-    double PreviewDistance;//预瞄距离
-
-    int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
-    int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
-    int conf =min(confL,confR);
-
-    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
-
-
-
-
-
-    if (realSpeed > 40)	KEang = 10; KEPos = 8;
-    if (realSpeed > 50) KEang = 5;
-
-    KEPos = 20;
-    KEang = 200;
-    //KEang = 15;
-
-    double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
-    double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
-    double a = ServiceCarStatus.Lane.curvature;
-    double b = ServiceCarStatus.Lane.heading;
-    double c = (c1+c2)*0.5;
-    double yaw= ServiceCarStatus.Lane.yaw;
-
-    double x= PreviewDistance;
-    double y;
-
-
-    y=c-(a*x*x+b*x);
-    double difa=0-(atan(2*a*x+b) / PI * 180);
-    Curve=0-a;
-
-    //EAng=difa;
-    //EPos=y;
-    EAng= 0-b;
-    EPos = c;
-    DEang = 10;
-    DEPos = 20;
-    //DEang = 20;
-    //DEPos = 10;
-
-    IEang = EAng+0.7*IEang;
-    IEPos = EPos+0.7*IEPos;
-    KIEang = 0;
-    //KIEang = 0.5;
-    KIEPos =2;
-
-
-
-    if(abs(confL)>=2&&abs(confR)>=2){
-        //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
-        ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
-    }else{
-        ang=lastAng;
-    }
-    //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;
-
-    lastEA = EAng;
-    lastEP = EPos;
-
-    if (ang > angleLimit) {
-        ang = angleLimit;
-    }
-    else if (ang < -angleLimit) {
-        ang = -angleLimit;
-    }
-    if (lastAng != iv::MaxValue) {
-        ang = 0.2 * lastAng + 0.8 * ang;
-        //ODS("lastAng：%d\n", lastAng);
-    }
-    lastAng = ang;
-    return ang;
-}
-
-
-
-double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
-
-    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
-    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-
-
-
-    double x_1 = pt.x;
-    double y_1 = pt.y;
-    double angle_1 = getQieXianAngle(nowGps,aimGps);
-    double x_2 = 0.0, y_2 = 0.0;
-    double steering_angle;
-    double l = 2.950;
-    double r =6;
-    double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
-    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
-    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
-    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
-    double g_1 = tan(angle_1);
-    double car_pos[3] = { x_1,y_1,g_1 };
-    double parking_pos[2] = { x_2,y_2 };
-    double g_3;
-    double t[4][2];
-    double p[4];
-    double  s1, s2; //切点与车起始位置的距离
-    double  min;
-    int  min_i;
-
-    //g_3 = 0 - 0.5775;
-    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
-    //交点
-    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
-    y_3 = y_1 - g_1 * x_1;
-    //圆心1
-    x_o_1 = r;
-    y_o_1 = g_3 * r + y_3;
-    //圆形1的切点1
-    x_t_1 = 0.0;
-    y_t_1 = g_3 * r + y_3;
-    //圆形1的切点2
-    if (g_1 == 0)
-    {
-        x_t_2 = r;
-        y_t_2 = y_1 - g_1 * x_1;
-    }
-    else
-    {
-        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
-        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;
-
-    }
-    //圆心2
-    x_o_2 = 0 - r;
-    y_o_2 = y_3 - g_3 * r;
-    //圆形2的切点1
-    x_t_3 = 0;
-    y_t_3 = y_3 - g_3 * r;
-    //圆形2的切点2
-    if (g_1 == 0)
-    {
-        x_t_4 = 0 - r;
-        y_t_4 = y_1 - g_1 * x_1;
-    }
-    else
-    {
-        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
-        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;
-
-    }
-    t[0][0] = x_t_1;
-    t[0][1] = y_t_1;
-    t[1][0] = x_t_2;
-    t[1][1] = y_t_2;
-    t[2][0] = x_t_3;
-    t[2][1] = y_t_3;
-    t[3][0] = x_t_4;
-    t[3][1] = y_t_4;
-    for (int i = 0; i < 4; i++)
-    {
-
-        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
-
-    }
-    min = p[0];
-    min_i = 0;
-    for (int i = 1; i < 4; i++)
-    {
-
-        if (p[i] < min)
-        {
-            min = p[i]; min_i = i;
-        }
-    }
-    if (min_i < 2)
-    {
-        x_o = x_o_1;
-        y_o = y_o_1;
-        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
-        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
-        if (s1 < s2)
-        {
-            x_t_n = x_t_1;
-            y_t_n = y_t_1;
-            x_t_f = x_t_2;
-            y_t_f = y_t_2;
-        }
-        else
-        {
-            x_t_n = x_t_2;
-            y_t_n = y_t_2;
-            x_t_f = x_t_1;
-            y_t_f = y_t_1;
-
-        }
-    }
-    else
-    {
-        x_o = x_o_2;
-        y_o = y_o_2;
-        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
-        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
-
-        if (s1 < s2)
-        {
-
-            x_t_n = x_t_3;
-            y_t_n = y_t_3;
-            x_t_f = x_t_4;
-            y_t_f = y_t_4;
-        }
-        else
-        {
-            x_t_n = x_t_4;
-            y_t_n = y_t_4;
-            x_t_f = x_t_3;
-            y_t_f = y_t_3;
-        }
-
-
-
-
-    }
-    steering_angle = atan2(l, r);
-
-    if (x_t_n < 0)
-    {
-        steering_angle = 0 - steering_angle;
-    }
-
-    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
-    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
-    bocheAngle = steering_angle*180/PI;
-
-    cout << "近切点：x_t_n=" << x_t_n << endl;
-    cout << "近切点：y_t_n=" << y_t_n << endl;
-    cout << "远切点：x_t_f=" << x_t_f << endl;
-    cout << "远切点：y_t_f=" << y_t_f << endl;
-    cout << "航向角：" << steering_angle << endl;
-
-
-    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
-    //        return 1;
-    //    }
-    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
-    double disA = GetDistance(aimGps,nowGps);
-    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
-        return 1;
-    }
-
-    return 0;
-
-}
-
-
-
-//返回垂直平分线的斜率
-double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
-    double angl, x_3, angle_3;
-    if (tan(angle_1 == 0))
-    {
-        if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
-        {
-            angle_3 = 0 - 1;
-        }
-        else
-        {
-            angle_3 = 1;
-        }
-    }
-    else
-    {
-        x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
-        angl = tan(angle_1);//车所在直线的斜率
-        if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
-        {
-            if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
-            {
-                if (angl < 0)
-                {
-                    angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-                else
-                {
-                    angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-
-
-
-            }
-
-        }
-        else//第二象限
-        {
-            if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
-            {
-                if (angl < 0)
-                {
-                    angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-                else
-                {
-                    angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
-                }
-
-            }
-        }
-    }
-
-    return angle_3;
-
-}
-
-
-
-double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
-    double heading = nowGps.ins_heading_angle *PI/180;
-    double x1 = nowGps.gps_x;
-    double y1 = nowGps.gps_y;
-    if (heading<=PI*0.5)
-    {
-        heading = 0.5*PI - heading;
-    }
-    else if (heading>PI*0.5 && heading<=PI*1.5) {
-        heading = 1.5*PI - heading;
-    }
-    else if (heading>PI*1.5) {
-        heading = 2.5*PI - heading;
-    }
-    double k1 = tan(heading);
-    double x = x1+10;
-    double y = k1 * x + y1 - (k1 * x1);
-    Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-    Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
-    double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
-    double angle = atan(abs(xielv));
-    if (xielv<0)
-    {
-        angle = PI - angle;
-    }
-    return angle;
-
-}
-
-
-/*
-  chuizhicheweiboche
-  */
-
-
-int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
-{
-
-
-
-    double l=2.95;//轴距
-    double x_0 = 0, y_0 = 0.5;
-    double x_1, y_1;//车起点坐标
-    double ange1;//车航向角弧度
-    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
-    double real_rad;;//另一条直线的航向角弧度
-    double angle_3;//垂直平分线弧度
-    double x_3, y_3;//垂直平分线交点
-    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
-    double x_o_1, y_o_1;//圆形1坐标
-    double x_o_2, y_o_2;//圆形2坐标
-    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
-    double min_rad;
-    double R_M; //后轴中点的转弯半径
-    double steering_angle;
-
-
-
-
-    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
-    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
-    x_1=pt.x;
-    y_1=pt.y;
-    ange1=getQieXianAngle(nowGps,aimGps);
-
-    min_rad_zhuanxiang(&R_M , &min_rad);
-    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
-    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
-    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
-    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
-    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
-                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
-    steering_angle = atan2(l, R_M);
-    x_4 = 0.5;
-    y_4 = 0;
-    //for (int i = 0; i < 4; i++)
-    //{
-    //for (int j = 0; j < 2; j++)
-    //{
-    //	cout << t[i][j] << endl;
-    //}
-    //}
-    //cout << "min_rad:" << min_rad<< endl;
-    //cout << "jiaodian：x=" << x_3 << endl;
-    //cout << "jiaodian：y=" << y_3 << endl;
-    // cout << "R-M：" << R_M << endl;
-    cout << "x_0:" << x_0 << endl;
-    cout << "y_0:" << y_0 << endl;
-    cout << "x_2:" << x_2 << endl;
-    cout << "y_2:" << y_2 << endl;
-    cout << "近切点：x_t_n="<< x_t_n << endl;
-    cout << "近切点：y_t_n=" << y_t_n << endl;
-    cout << "远切点：x_t_f=" << x_t_f << endl;
-    cout << "远切点：y_t_f=" << y_t_f << endl;
-    //cout << "航向角：" << steering_angle << endl;
-    //cout << "圆心1横坐标=" << x_o_1 << endl;
-    //cout << "圆心1纵坐标=" << y_o_1 << endl;
-    //cout << "圆心2横坐标=" << x_o_2 << endl;
-    //cout << "圆心2纵坐标=" << y_o_2 << endl;
-    //cout << "平分线弧度=" << angle_3 << endl;
-    //cout << " min_rad=" << min_rad << endl;
-    //cout << " real_rad=" << real_rad << endl;
-    //   system("PAUSE");
-
-
-
-    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
-    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
-    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
-    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
-    dBocheAngle = steering_angle*180/PI;
-
-    double disA = GetDistance(aimGps,nowGps);
-
-    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
-        return 1;
-    }
-    return 0;
-
-}
-
-
-double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
-    double L_c = 4.749;//车长
-    double rad_1;
-    double rad_2;
-    double L_k = 1.931;//车宽
-    double L = 2.95;//轴距
-    double L_f =1.2 ;//前悬
-    double L_r =0.7 ;//后悬
-    double R_min =6.5 ;//最小转弯半径
-    *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
-    //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
-    //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
-    *min_rad = 45 * PI / 180;// rad_1 - rad_2;
-    return 0;
-}
-
-
-double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {
-
-    if (x_1 > 0 && y_1 > 0)
-    {
-        *real_rad = PI*0.5 - min_rad;
-        *x_2 = R_M - R_M*cos(min_rad);
-        *y_2 = R_M*sin(min_rad) + 0.5;
-    }
-    else
-    {
-        *real_rad = PI*0.5 + min_rad;
-        *x_2 = R_M*cos(min_rad) - R_M;
-        *y_2 = R_M*sin(min_rad) + 0.5;
-    }
-    return 0;
-
-}
-
-
-double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
-    double b1, b2;
-    double k1, k2;
-    if (ange1!=(PI*0.5))
-    {
-        k1 = tan(ange1);
-        b1 = y_1 - k1*x_1;
-        k2 = tan(real_rad);
-        b2 = y_2 - k2*x_2;
-        *x_3 = (b2 - b1) / (k1 - k2);
-        *y_3 = k2*(*x_3) + b2;
-    }
-    else
-    {
-        k2 = tan(real_rad);
-        b2 = y_2 - k2*x_2;
-        *x_3 = x_1;
-        *y_3 = k2*(*x_3) + b2;
-    }
-    return 0;
-}
-
-
-double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
-    double k1, k2;
-    double  angle_j;
-    k2 = tan(real_rad);
-    if (ange1 != (PI*0.5))
-    {
-        k1 = tan(ange1);
-        angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角
-
-        if (x_1 > 0 && y_1 > 0)
-        {
-            *angle_3 = angle_j*0.5 - min_rad + PI;
-        }
-        else
-        {
-            *angle_3 = min_rad - angle_j*0.5;
-        }
-    }
-    else
-    {
-        angle_j = min_rad;//两直线夹角
-        if (x_1 > 0 && y_1 > 0)
-        {
-            *angle_3 = angle_j*0.5 - min_rad + PI;
-        }
-        else
-        {
-            *angle_3 = min_rad - angle_j*0.5;
-        }
-    }
-    return 0;
-}
-
-
-double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
-                                        double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
-    double b2, b3, k2, k3;
-    b2 = y_2 - tan(real_rad)*x_2;
-    b3 = y_3 - tan(angle_3)*x_3;
-    k2 = tan(real_rad);
-    k3 = tan(angle_3);
-    *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
-    *y_o_1 = k3*(*x_o_1) + b3;
-
-    *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
-    *y_o_2 = k3*(*x_o_2) + b3;
-    return 0;
-}
-
-
-double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
-                                                double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
-                                                double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
-{
-    double x_o, y_o;
-    double b2, b3, k1, k2, k3;
-    //double car_pos[3] = { x_1,y_1,k1 };
-    double parking_pos[2] = { x_2,y_2 };
-    //double t[4][2];
-    double p[4];
-    double  s1, s2; //切点与车起始位置的距离
-    double  min;
-    int  min_i;
-    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
-    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
-    double t[4][2];
-    k1 = tan(ange1);
-    b2 = y_2 - tan(real_rad)*x_2;
-    b3 = y_3 - tan(real_rad)*x_3;
-    k2 = tan(real_rad);//另一条直线斜率
-    k3 = tan(angle_3);//垂直平分线斜率
-    //圆心1和2切点*********************************************
-    if (x_1 > 0 && y_1 > 0)//第一象限
-    {
-        if (ange1 == (PI*0.5))
-        {
-            x_t_1 = x_1;
-            y_t_1 = y_o_1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            x_t_3 = x_1;
-            y_t_3 = y_o_2;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-
-        }
-        else
-        {
-            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-        }
-    }
-    else
-    {
-        if (ange1 == 0)
-        {
-            x_t_1 = 0 - x_1;
-            y_t_1 = y_o_1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            x_t_3 = 0 - x_1;
-            y_t_3 = y_o_2;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-        }
-        else
-        {
-            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
-            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
-
-            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
-            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
-            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
-            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
-
-        }
-
-    }
-
-    //圆心1和2切点*********************************************
-
-    t[0][0] = x_t_1;
-    t[0][1] = y_t_1;
-    t[1][0] = x_t_2;
-    t[1][1] = y_t_2;
-    t[2][0] = x_t_3;
-    t[2][1] = y_t_3;
-    t[3][0] = x_t_4;
-    t[3][1] = y_t_4;
-    for (int i = 0; i < 4; i++)
-    {
-
-        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
-
-    }
-    min = p[0];
-    min_i = 0;
-    for (int i = 1; i < 4; i++)
-    {
-
-        if (p[i] < min)
-        {
-            min = p[i]; min_i = i;
-        }
-    }
-    if (min_i < 2)
-    {
-        x_o = x_o_1;
-        y_o = y_o_1;
-        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
-        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
-        if (s1 < s2)
-        {
-            *x_t_n = x_t_1;
-            *y_t_n = y_t_1;
-            *x_t_f = x_t_2;
-            *y_t_f = y_t_2;
-        }
-        else
-        {
-            *x_t_n = x_t_2;
-            *y_t_n = y_t_2;
-            *x_t_f = x_t_1;
-            *y_t_f = y_t_1;
-
-        }
-    }
-    else
-    {
-        x_o = x_o_2;
-        y_o = y_o_2;
-        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
-        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
-
-        if (s1 < s2)
-        {
-
-            *x_t_n = x_t_3;
-            *y_t_n = y_t_3;
-            *x_t_f = x_t_4;
-            *y_t_f = y_t_4;
-        }
-        else
-        {
-            *x_t_n = x_t_4;
-            *y_t_n = y_t_4;
-            *x_t_f = x_t_3;
-            *y_t_f = y_t_3;
-        }
-
-
-
-    }
-
-    return 0;
-
-}
-
-
-int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
-{
-    int index = -1;
-    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
-    int endIndex = gpsMap.size() - 1;
-    float minDis=20;
-
-    for (int j = startIndex; j < endIndex; j++)
-    {
-        int i = (j + gpsMap.size()) % gpsMap.size();
-        double tmpdis = GetDistance(rp, (*gpsMap[i]));
-
-        if (tmpdis < minDis)
-        {
-            index = i;
-            minDis=tmpdis;
-        }
-    }
-    return index;
-}
-
-
-double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D>& gpsTrace, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {
-    double obsSpeed = 0 - realSecSpeed;
-    double minDis = iv::MaxValue;
-    FrenetPoint esr_obs_F_point;
-    for (int i = 0; i < 64; i++)
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
-        {
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
-
-            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
-            {
-                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
-                if (tmpDis < minDis)
-                {
-                    minDis = tmpDis;
-//                    esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
-                    esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
-//                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
-                    double speedx=ServiceCarStatus.obs_radar[i].speed_x;  //障碍物相对于车辆x轴的速度
-                    double speedy=ServiceCarStatus.obs_radar[i].speed_y;  //障碍物相对于车辆y轴的速度
-                    double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
-                    //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
-                    //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
-                    double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);
-
-                    obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
-                }
-            }
-        }
-
-    return obsSpeed;
-}
-
-
-int iv::decition::Compute00::getEsrIndexByFrenet(const std::vector<Point2D>& gpsTrace, FrenetPoint &esrObsPoint, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps){
-    double minDistance = numeric_limits<double>::max();
-    int minDis_index=-1;
-
-    for(int i=0; i<64; ++i){
-        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
-            //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
-            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
-            double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
-
-            //将毫米波障碍物位置转换到frenet坐标系下
-//            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
-            esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
-
-            //如果障碍物与道路的横向距离d<=3.0*Veh_Width / 4.0，则认为道路上有障碍物。
-            //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
-            //minDistance、minDis_index用来统计最近的障碍物信息。
-            if(abs(esrObsPoint.d)<=(3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs)){
-                if(esrObsPoint.s<minDistance){
-                    minDistance = esrObsPoint.s;
-                    minDis_index = i;
-                }
-            }
-        }
-    }
-    return minDis_index;
-}
-
-
-
-
-std::vector<std::vector<iv::GPSData>> gmapsL;
-std::vector<std::vector<iv::GPSData>> gmapsR;
+#include <decition/compute_00.h>
+#include <decition/decide_gps_00.h>
+#include <decition/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/transfer.h>
+#include <common/constants.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+//#include <control/radar_exam.h>
+#include <control/can.h>
+using namespace std;
+
+#define PI (3.1415926535897932384626433832795)
+
+iv::decition::Compute00::Compute00() {
+
+}
+iv::decition::Compute00::~Compute00() {
+
+}
+
+
+double iv::decition::Compute00::angleLimit = 700;
+double iv::decition::Compute00::lastEA = 0;
+double iv::decition::Compute00::lastEP = 0;
+double iv::decition::Compute00::decision_Angle = 0;
+double iv::decition::Compute00::lastAng = 0;
+int iv::decition::Compute00::lastEsrID = -1;
+int  iv::decition::Compute00::lastEsrCount = 0;
+int iv::decition::Compute00::lastEsrIDAvoid = -1;
+int  iv::decition::Compute00::lastEsrCountAvoid = 0;
+
+iv::GPS_INS  iv::decition::Compute00::nearTpoint;
+iv::GPS_INS  iv::decition::Compute00::farTpoint;
+double  iv::decition::Compute00::bocheAngle;
+
+
+
+iv::GPS_INS  iv::decition::Compute00::dTpoint0;
+iv::GPS_INS  iv::decition::Compute00::dTpoint1;
+iv::GPS_INS  iv::decition::Compute00::dTpoint2;
+iv::GPS_INS  iv::decition::Compute00::dTpoint3;
+double  iv::decition::Compute00::dBocheAngle;
+
+
+std::vector<int> lastEsrIdVector;
+std::vector<int> lastEsrCountVector;
+
+int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
+{
+    int index = -1;
+    //	DecideGps00().minDis = iv::MaxValue;
+    DecideGps00().minDis = 20;
+    DecideGps00().maxAngle = iv::MaxValue;
+
+    int startIndex = max((lastIndex - 500), 0);     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = min((gpsMap.size() - 1), (size_t)(lastIndex + 2000 + 100 * (gpsMissCount + 1)));
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < DecideGps00().minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
+                )
+        {
+            index = i;
+            DecideGps00().minDis = tmpdis;
+            DecideGps00().maxAngle = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
+            DecideGps00().maxAngle = min(DecideGps00().maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+        }
+    }
+    /*if (index == -1) {
+        index = 0;
+    }*/
+    return index;
+}
+
+
+//首次找点
+
+int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
+{
+    int index = -1;
+    //	DecideGps00().minDis = iv::MaxValue;
+    DecideGps00().minDis = 40;
+    DecideGps00().maxAngle = iv::MaxValue;
+
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < DecideGps00().minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
+                )
+        {
+            index = i;
+            DecideGps00().minDis = tmpdis;
+            DecideGps00().maxAngle = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
+            DecideGps00().maxAngle = min(DecideGps00().maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+        }
+    }
+    /*if (index == -1) {
+        index = 0;
+    }*/
+    return index;
+}
+
+
+
+
+
+double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
+{
+    double sum_x = 0;
+    double sum_y = 0;
+
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+
+
+    return atan(average_x / average_y) / PI * 180;
+}
+
+
+
+double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
+{
+    double sum_x = 0;
+    double sum_y = 0;
+
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+
+
+    return atan(average_x + avoidX / average_y) / PI * 180;
+}
+
+
+
+
+
+
+double iv::decition::Compute00::getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    //   double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1;  // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+
+    if(transferPieriod&& !transferPieriod2){
+        DEang = 200;
+        DEPos = 150;
+    }
+
+    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+    double PreviewDistance;//预瞄距离
+    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+    if(changeRoad ||transferPieriod){
+        PreviewDistance=PreviewDistance+avoidX;
+    }
+    if(realSpeed <15){
+        PreviewDistance = max(4.0, realSpeed *0.4) ;
+    }
+
+    if (gpsTrace[0].v1 == 1)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
+    {
+        KEang = 18; KEPos = 50; PreviewDistance = 3;
+    }
+    else if (gpsTrace[0].v1 == 7)
+    {
+        KEang = 20; KEPos = 50; PreviewDistance = 4;
+    }
+
+
+    if (realSpeed > 40)	KEang = 10; KEPos = 8;
+    if (realSpeed > 50) KEang = 5;
+
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+
+
+
+
+
+    for (int i = 1; i < gpsTrace.size() - 1; i++)
+    {
+        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+
+
+
+
+    EPos = gpsTrace[gpsIndex].x;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
+        farTrace.push_back(gpsTrace[gpsIndex]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAveDef(farTrace);
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+    return ang;
+}
+
+
+
+
+int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
+{
+    int index = 1;
+    double sumdis = 0;
+    while (index < gpsTrace.size() && sumdis < realSpeed)
+        sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);
+
+    if (index == gpsTrace.size())
+        return index - 1;
+
+    if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
+        index--;
+    return index;
+}
+
+iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + Veh_Width / 2 * cos(anglevalue + PI / 2),
+                       carFronty + Veh_Width / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + Veh_Width / 2 * cos(anglevalue - PI / 2),
+                        carFronty + Veh_Width / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>2.5 )
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                //		DecideGps00().lidarDistance = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistance = obsPoint.y;
+    return obsPoint;
+}
+
+//1220
+iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue + PI / 2),
+                       carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue - PI / 2),
+                        carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y<-1.0 )
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+
+                dx=grx-dx;//1227
+
+                int dy = (pty + gridwide*(double)centery)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+
+                dx=grx-dx;//1227
+                int dy = (pty + gridwide*(double)centery)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                //		DecideGps00().lidarDistance = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistance = obsPoint.y;
+    return obsPoint;
+}
+
+
+iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        //1127 fanwei xiuzheng
+        float buchang=0;
+        Point2D ptLeft(carFrontx + (Veh_Width+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
+                       carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx +  (Veh_Width+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
+                        carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
+                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                DecideGps00().lidarDistanceAvoid = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistanceAvoid = obsPoint.y;
+
+    return obsPoint;
+}
+
+
+
+
+//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
+//	bool isRemove = false;
+//
+//	for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+//	{
+//
+//		for (int i = 0; i < esrRadars.size(); i++)
+//			if ((esrRadars[i].nomal_y) != 0)
+//			{
+//				double xxx = esrRadars[i].nomal_x + Esr_Offset;
+//				double yyy = esrRadars[i].nomal_y;
+//
+//				if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//				{
+//
+//					if (lastEsrID == (esrRadars[i]).esr_ID)
+//					{
+//						lastEsrCount++;
+//					}
+//					else
+//					{
+//						lastEsrCount = 0;
+//					}
+//
+//					if (lastEsrCount >= 3)
+//					{
+//						return i;
+//					}
+//
+//					lastEsrID = (esrRadars[i]).esr_ID;
+//				}
+//			}
+//	}
+//	return -1;
+//}
+
+
+
+
+int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint) {
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
+            {
+                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+                double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+//优化
+//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*Veh_Width / 2.0+DecideGps00().xiuzhengCs)){
+//                    *esrPathpoint = j;
+//                    return i;
+//                }
+
+
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrID == i)
+                    {
+                        lastEsrCount++;
+                    }
+                    else
+                    {
+                        lastEsrCount = 0;
+                    }
+
+                    if(yyy>50 ){
+                        if (lastEsrCount >=200)
+                        {
+                            return i;
+                        }
+                    }
+                    else if (lastEsrCount >= 1)
+                    {
+                        return i;
+                    }
+
+                    lastEsrID = i;
+                }
+            }
+    }
+    return -1;
+}
+
+//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
+//    bool isRemove = false;
+
+//    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+//    {
+
+//        for (int i = 0; i < 64; i++)
+//            if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
+//            {
+//                double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
+//                double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+//                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+//                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+
+//                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//                {
+
+//                    if (lastEsrID == i)
+//                    {
+//                        lastEsrCount++;
+//                    }
+//                    else
+//                    {
+//                        lastEsrCount = 0;
+//                    }
+
+//                    if(yyy>50 ){
+//                        if (lastEsrCount >=200)
+//                        {
+//                            return i;
+//                        }
+//                    }
+//                    else if (lastEsrCount >= 3)
+//                    {
+//                        return i;
+//                    }
+
+//                    lastEsrID = i;
+//                }
+//            }
+//    }
+//    return -1;
+//}
+
+
+
+int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
+            {
+                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+                double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
+
+                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrIDAvoid == i)
+                    {
+                        lastEsrCountAvoid++;
+                    }
+                    else
+                    {
+                        lastEsrCountAvoid = 0;
+                    }
+
+                    if (lastEsrCountAvoid >= 6)
+                    {
+                        return i;
+                    }
+
+                    lastEsrIDAvoid = i;
+                }
+            }
+    }
+    return -1;
+}
+
+
+
+
+
+
+
+//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
+//	double obsSpeed = 0 - realSecSpeed;
+//	double minDis = iv::MaxValue;
+//	for (int i = 0; i < esrRadars.size(); i++)
+//		if ((esrRadars[i].nomal_y) != 0)
+//		{
+//			double xxx = esrRadars[i].nomal_x + Esr_Offset;
+//			double yyy = esrRadars[i].nomal_y;
+//
+//			if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+//			{
+//				double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+//				if (tmpDis < minDis)
+//				{
+//					minDis = tmpDis;
+//					obsSpeed = esrRadars[i].speed_y;
+//				}
+//			}
+//		}
+//
+//	return obsSpeed;
+//
+//
+//}
+
+
+
+
+double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
+    double obsSpeed = 0 - realSecSpeed;
+    double minDis = iv::MaxValue;
+    for (int i = 0; i < 64; i++)
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
+        {
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
+
+            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+            {
+                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+                if (tmpDis < minDis)
+                {
+                    minDis = tmpDis;
+                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
+                }
+            }
+        }
+
+    return obsSpeed;
+}
+
+
+
+
+double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+
+    double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+
+    if (gpsTrace[0].v1 == 1)
+    {
+        KEang = 10; KEPos = 8;
+        if (realSpeed > 60) KEang = 5;
+    }
+    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
+    {
+        KEang = 18; KEPos = 50; PreviewDistance = 3;
+    }
+    else if (gpsTrace[0].v1 == 7)
+    {
+        KEang = 20; KEPos = 50; PreviewDistance = 4;
+    }
+
+
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+
+
+
+
+
+    for (int i = 1; i < gpsTrace.size() - 1; i++)
+    {
+        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+
+    if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
+    {
+        gpsIndex = DecideGps00().gpsLineParkIndex;
+    }
+
+
+
+    EPos = gpsTrace[gpsIndex].x + avoidX;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
+        farTrace.push_back(gpsTrace[gpsIndex]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAvoidAveDef(farTrace, avoidX);
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+
+    return ang;
+}
+
+
+std::vector<iv::GPSData>   iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {
+
+    vector<vector<iv::GPSData>> maps;
+    vector<iv::GPSData> gpsMapLineBeside;
+    int sizeN = gpsMapLine.size();
+    for (int i = 1; i < sizeN; i++)
+    {
+        iv::GPSData gpsData(new GPS_INS);
+        double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
+        double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
+        double lng = ServiceCarStatus.location->ins_heading_angle;
+
+
+        double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
+        double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
+        double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
+        double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
+
+        //	memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));
+
+        gpsData->speed_mode = gpsMapLine[i]->speed_mode;
+        gpsData->gps_x = x0 + k1 * avoidX;
+        gpsData->gps_y = y0 + k2 * avoidX;
+        gpsMapLineBeside.push_back(gpsData);
+
+    }
+    return gpsMapLineBeside;
+
+}
+
+
+
+//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {
+
+//    double ang = 0;
+//    double EPos = 0, EAng = 0;
+
+// //   double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
+//       double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;
+
+// //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+//    double PreviewDistance;//预瞄距离
+//    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+
+
+
+
+////    if (realSpeed > 40)	KEang = 10; KEpos = 8;
+////        if (realSpeed > 50) KEang = 5;
+
+
+
+
+
+//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
+//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
+//double a = ServiceCarStatus.Lane.curvature;
+//double b = ServiceCarStatus.Lane.heading;
+//double c = (c1+c2)*0.5;
+//double x= PreviewDistance;
+//double y;
+
+
+//y=a*x*x+b*x+c;
+
+//   // EPos = y;
+//EPos=c;
+
+
+//  //  EAng=atan(2*a*x+b) / PI * 180;
+//    EAng=ServiceCarStatus.Lane.yaw;
+//        ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+//        lastEA = EAng;
+//        lastEP = EPos;
+
+
+//           std::cout << "\nEPos:%f\n" << EPos << std::endl;
+//            std::cout << "\nEAng:%f\n" << EAng << std::endl;
+
+
+//        if (ang > angleLimit) {
+//            ang = angleLimit;
+//        }
+//        else if (ang < -angleLimit) {
+//            ang = -angleLimit;
+//        }
+//        if (lastAng != iv::MaxValue) {
+//            ang = 0.2 * lastAng + 0.8 * ang;
+//            //ODS("lastAng：%d\n", lastAng);
+//        }
+//        lastAng = ang;
+//        return ang;
+//    }
+
+
+
+double IEPos = 0, IEang = 0;
+
+
+double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+    double Curve=0;
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+    double KCurve=120;
+    double KIEPos = 0, KIEang = 0;
+    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+    double PreviewDistance;//预瞄距离
+
+    int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
+    int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
+    int conf =min(confL,confR);
+
+    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+
+
+
+
+    if (realSpeed > 40)	KEang = 10; KEPos = 8;
+    if (realSpeed > 50) KEang = 5;
+
+    KEPos = 20;
+    KEang = 200;
+    //KEang = 15;
+
+    double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
+    double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
+    double a = ServiceCarStatus.Lane.curvature;
+    double b = ServiceCarStatus.Lane.heading;
+    double c = (c1+c2)*0.5;
+    double yaw= ServiceCarStatus.Lane.yaw;
+
+    double x= PreviewDistance;
+    double y;
+
+
+    y=c-(a*x*x+b*x);
+    double difa=0-(atan(2*a*x+b) / PI * 180);
+    Curve=0-a;
+
+    //EAng=difa;
+    //EPos=y;
+    EAng= 0-b;
+    EPos = c;
+    DEang = 10;
+    DEPos = 20;
+    //DEang = 20;
+    //DEPos = 10;
+
+    IEang = EAng+0.7*IEang;
+    IEPos = EPos+0.7*IEPos;
+    KIEang = 0;
+    //KIEang = 0.5;
+    KIEPos =2;
+
+
+
+    if(abs(confL)>=2&&abs(confR)>=2){
+        //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
+        ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
+    }else{
+        ang=lastAng;
+    }
+    //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+    return ang;
+}
+
+
+
+double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
+
+    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
+    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+
+
+
+    double x_1 = pt.x;
+    double y_1 = pt.y;
+    double angle_1 = getQieXianAngle(nowGps,aimGps);
+    double x_2 = 0.0, y_2 = 0.0;
+    double steering_angle;
+    double l = 2.950;
+    double r =6;
+    double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
+    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
+    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
+    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
+    double g_1 = tan(angle_1);
+    double car_pos[3] = { x_1,y_1,g_1 };
+    double parking_pos[2] = { x_2,y_2 };
+    double g_3;
+    double t[4][2];
+    double p[4];
+    double  s1, s2; //切点与车起始位置的距离
+    double  min;
+    int  min_i;
+
+    //g_3 = 0 - 0.5775;
+    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
+    //交点
+    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
+    y_3 = y_1 - g_1 * x_1;
+    //圆心1
+    x_o_1 = r;
+    y_o_1 = g_3 * r + y_3;
+    //圆形1的切点1
+    x_t_1 = 0.0;
+    y_t_1 = g_3 * r + y_3;
+    //圆形1的切点2
+    if (g_1 == 0)
+    {
+        x_t_2 = r;
+        y_t_2 = y_1 - g_1 * x_1;
+    }
+    else
+    {
+        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
+        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;
+
+    }
+    //圆心2
+    x_o_2 = 0 - r;
+    y_o_2 = y_3 - g_3 * r;
+    //圆形2的切点1
+    x_t_3 = 0;
+    y_t_3 = y_3 - g_3 * r;
+    //圆形2的切点2
+    if (g_1 == 0)
+    {
+        x_t_4 = 0 - r;
+        y_t_4 = y_1 - g_1 * x_1;
+    }
+    else
+    {
+        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
+        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;
+
+    }
+    t[0][0] = x_t_1;
+    t[0][1] = y_t_1;
+    t[1][0] = x_t_2;
+    t[1][1] = y_t_2;
+    t[2][0] = x_t_3;
+    t[2][1] = y_t_3;
+    t[3][0] = x_t_4;
+    t[3][1] = y_t_4;
+    for (int i = 0; i < 4; i++)
+    {
+
+        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
+
+    }
+    min = p[0];
+    min_i = 0;
+    for (int i = 1; i < 4; i++)
+    {
+
+        if (p[i] < min)
+        {
+            min = p[i]; min_i = i;
+        }
+    }
+    if (min_i < 2)
+    {
+        x_o = x_o_1;
+        y_o = y_o_1;
+        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
+        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
+        if (s1 < s2)
+        {
+            x_t_n = x_t_1;
+            y_t_n = y_t_1;
+            x_t_f = x_t_2;
+            y_t_f = y_t_2;
+        }
+        else
+        {
+            x_t_n = x_t_2;
+            y_t_n = y_t_2;
+            x_t_f = x_t_1;
+            y_t_f = y_t_1;
+
+        }
+    }
+    else
+    {
+        x_o = x_o_2;
+        y_o = y_o_2;
+        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
+        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
+
+        if (s1 < s2)
+        {
+
+            x_t_n = x_t_3;
+            y_t_n = y_t_3;
+            x_t_f = x_t_4;
+            y_t_f = y_t_4;
+        }
+        else
+        {
+            x_t_n = x_t_4;
+            y_t_n = y_t_4;
+            x_t_f = x_t_3;
+            y_t_f = y_t_3;
+        }
+
+
+
+
+    }
+    steering_angle = atan2(l, r);
+
+    if (x_t_n < 0)
+    {
+        steering_angle = 0 - steering_angle;
+    }
+
+    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
+    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
+    bocheAngle = steering_angle*180/PI;
+
+    cout << "近切点：x_t_n=" << x_t_n << endl;
+    cout << "近切点：y_t_n=" << y_t_n << endl;
+    cout << "远切点：x_t_f=" << x_t_f << endl;
+    cout << "远切点：y_t_f=" << y_t_f << endl;
+    cout << "航向角：" << steering_angle << endl;
+
+
+    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
+    //        return 1;
+    //    }
+    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
+    double disA = GetDistance(aimGps,nowGps);
+    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
+        return 1;
+    }
+
+    return 0;
+
+}
+
+
+
+//返回垂直平分线的斜率
+double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
+    double angl, x_3, angle_3;
+    if (tan(angle_1 == 0))
+    {
+        if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
+        {
+            angle_3 = 0 - 1;
+        }
+        else
+        {
+            angle_3 = 1;
+        }
+    }
+    else
+    {
+        x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
+        angl = tan(angle_1);//车所在直线的斜率
+        if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
+        {
+            if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
+            {
+                if (angl < 0)
+                {
+                    angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+                else
+                {
+                    angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+
+
+
+            }
+
+        }
+        else//第二象限
+        {
+            if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
+            {
+                if (angl < 0)
+                {
+                    angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+                else
+                {
+                    angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+
+            }
+        }
+    }
+
+    return angle_3;
+
+}
+
+
+
+double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
+    double heading = nowGps.ins_heading_angle *PI/180;
+    double x1 = nowGps.gps_x;
+    double y1 = nowGps.gps_y;
+    if (heading<=PI*0.5)
+    {
+        heading = 0.5*PI - heading;
+    }
+    else if (heading>PI*0.5 && heading<=PI*1.5) {
+        heading = 1.5*PI - heading;
+    }
+    else if (heading>PI*1.5) {
+        heading = 2.5*PI - heading;
+    }
+    double k1 = tan(heading);
+    double x = x1+10;
+    double y = k1 * x + y1 - (k1 * x1);
+    Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+    Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
+    double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
+    double angle = atan(abs(xielv));
+    if (xielv<0)
+    {
+        angle = PI - angle;
+    }
+    return angle;
+
+}
+
+
+/*
+  chuizhicheweiboche
+  */
+
+
+int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
+{
+
+
+
+    double l=2.95;//轴距
+    double x_0 = 0, y_0 = 0.5;
+    double x_1, y_1;//车起点坐标
+    double ange1;//车航向角弧度
+    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
+    double real_rad;;//另一条直线的航向角弧度
+    double angle_3;//垂直平分线弧度
+    double x_3, y_3;//垂直平分线交点
+    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
+    double x_o_1, y_o_1;//圆形1坐标
+    double x_o_2, y_o_2;//圆形2坐标
+    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
+    double min_rad;
+    double R_M; //后轴中点的转弯半径
+    double steering_angle;
+
+
+
+
+    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
+    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+    x_1=pt.x;
+    y_1=pt.y;
+    ange1=getQieXianAngle(nowGps,aimGps);
+
+    min_rad_zhuanxiang(&R_M , &min_rad);
+    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
+    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
+    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
+    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
+    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
+                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
+    steering_angle = atan2(l, R_M);
+    x_4 = 0.5;
+    y_4 = 0;
+    //for (int i = 0; i < 4; i++)
+    //{
+    //for (int j = 0; j < 2; j++)
+    //{
+    //	cout << t[i][j] << endl;
+    //}
+    //}
+    //cout << "min_rad:" << min_rad<< endl;
+    //cout << "jiaodian：x=" << x_3 << endl;
+    //cout << "jiaodian：y=" << y_3 << endl;
+    // cout << "R-M：" << R_M << endl;
+    cout << "x_0:" << x_0 << endl;
+    cout << "y_0:" << y_0 << endl;
+    cout << "x_2:" << x_2 << endl;
+    cout << "y_2:" << y_2 << endl;
+    cout << "近切点：x_t_n="<< x_t_n << endl;
+    cout << "近切点：y_t_n=" << y_t_n << endl;
+    cout << "远切点：x_t_f=" << x_t_f << endl;
+    cout << "远切点：y_t_f=" << y_t_f << endl;
+    //cout << "航向角：" << steering_angle << endl;
+    //cout << "圆心1横坐标=" << x_o_1 << endl;
+    //cout << "圆心1纵坐标=" << y_o_1 << endl;
+    //cout << "圆心2横坐标=" << x_o_2 << endl;
+    //cout << "圆心2纵坐标=" << y_o_2 << endl;
+    //cout << "平分线弧度=" << angle_3 << endl;
+    //cout << " min_rad=" << min_rad << endl;
+    //cout << " real_rad=" << real_rad << endl;
+    //   system("PAUSE");
+
+
+
+    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
+    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
+    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
+    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
+    dBocheAngle = steering_angle*180/PI;
+
+    double disA = GetDistance(aimGps,nowGps);
+
+    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
+        return 1;
+    }
+    return 0;
+
+}
+
+
+double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
+    double L_c = 4.749;//车长
+    double rad_1;
+    double rad_2;
+    double L_k = 1.931;//车宽
+    double L = 2.95;//轴距
+    double L_f =1.2 ;//前悬
+    double L_r =0.7 ;//后悬
+    double R_min =6.5 ;//最小转弯半径
+    *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
+    //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
+    //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
+    *min_rad = 45 * PI / 180;// rad_1 - rad_2;
+    return 0;
+}
+
+
+double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {
+
+    if (x_1 > 0 && y_1 > 0)
+    {
+        *real_rad = PI*0.5 - min_rad;
+        *x_2 = R_M - R_M*cos(min_rad);
+        *y_2 = R_M*sin(min_rad) + 0.5;
+    }
+    else
+    {
+        *real_rad = PI*0.5 + min_rad;
+        *x_2 = R_M*cos(min_rad) - R_M;
+        *y_2 = R_M*sin(min_rad) + 0.5;
+    }
+    return 0;
+
+}
+
+
+double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
+    double b1, b2;
+    double k1, k2;
+    if (ange1!=(PI*0.5))
+    {
+        k1 = tan(ange1);
+        b1 = y_1 - k1*x_1;
+        k2 = tan(real_rad);
+        b2 = y_2 - k2*x_2;
+        *x_3 = (b2 - b1) / (k1 - k2);
+        *y_3 = k2*(*x_3) + b2;
+    }
+    else
+    {
+        k2 = tan(real_rad);
+        b2 = y_2 - k2*x_2;
+        *x_3 = x_1;
+        *y_3 = k2*(*x_3) + b2;
+    }
+    return 0;
+}
+
+
+double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
+    double k1, k2;
+    double  angle_j;
+    k2 = tan(real_rad);
+    if (ange1 != (PI*0.5))
+    {
+        k1 = tan(ange1);
+        angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角
+
+        if (x_1 > 0 && y_1 > 0)
+        {
+            *angle_3 = angle_j*0.5 - min_rad + PI;
+        }
+        else
+        {
+            *angle_3 = min_rad - angle_j*0.5;
+        }
+    }
+    else
+    {
+        angle_j = min_rad;//两直线夹角
+        if (x_1 > 0 && y_1 > 0)
+        {
+            *angle_3 = angle_j*0.5 - min_rad + PI;
+        }
+        else
+        {
+            *angle_3 = min_rad - angle_j*0.5;
+        }
+    }
+    return 0;
+}
+
+
+double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
+                                        double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
+    double b2, b3, k2, k3;
+    b2 = y_2 - tan(real_rad)*x_2;
+    b3 = y_3 - tan(angle_3)*x_3;
+    k2 = tan(real_rad);
+    k3 = tan(angle_3);
+    *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
+    *y_o_1 = k3*(*x_o_1) + b3;
+
+    *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
+    *y_o_2 = k3*(*x_o_2) + b3;
+    return 0;
+}
+
+
+double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
+                                                double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
+                                                double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
+{
+    double x_o, y_o;
+    double b2, b3, k1, k2, k3;
+    //double car_pos[3] = { x_1,y_1,k1 };
+    double parking_pos[2] = { x_2,y_2 };
+    //double t[4][2];
+    double p[4];
+    double  s1, s2; //切点与车起始位置的距离
+    double  min;
+    int  min_i;
+    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
+    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
+    double t[4][2];
+    k1 = tan(ange1);
+    b2 = y_2 - tan(real_rad)*x_2;
+    b3 = y_3 - tan(real_rad)*x_3;
+    k2 = tan(real_rad);//另一条直线斜率
+    k3 = tan(angle_3);//垂直平分线斜率
+    //圆心1和2切点*********************************************
+    if (x_1 > 0 && y_1 > 0)//第一象限
+    {
+        if (ange1 == (PI*0.5))
+        {
+            x_t_1 = x_1;
+            y_t_1 = y_o_1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            x_t_3 = x_1;
+            y_t_3 = y_o_2;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+
+        }
+        else
+        {
+            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+        }
+    }
+    else
+    {
+        if (ange1 == 0)
+        {
+            x_t_1 = 0 - x_1;
+            y_t_1 = y_o_1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            x_t_3 = 0 - x_1;
+            y_t_3 = y_o_2;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+        }
+        else
+        {
+            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+        }
+
+    }
+
+    //圆心1和2切点*********************************************
+
+    t[0][0] = x_t_1;
+    t[0][1] = y_t_1;
+    t[1][0] = x_t_2;
+    t[1][1] = y_t_2;
+    t[2][0] = x_t_3;
+    t[2][1] = y_t_3;
+    t[3][0] = x_t_4;
+    t[3][1] = y_t_4;
+    for (int i = 0; i < 4; i++)
+    {
+
+        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
+
+    }
+    min = p[0];
+    min_i = 0;
+    for (int i = 1; i < 4; i++)
+    {
+
+        if (p[i] < min)
+        {
+            min = p[i]; min_i = i;
+        }
+    }
+    if (min_i < 2)
+    {
+        x_o = x_o_1;
+        y_o = y_o_1;
+        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
+        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
+        if (s1 < s2)
+        {
+            *x_t_n = x_t_1;
+            *y_t_n = y_t_1;
+            *x_t_f = x_t_2;
+            *y_t_f = y_t_2;
+        }
+        else
+        {
+            *x_t_n = x_t_2;
+            *y_t_n = y_t_2;
+            *x_t_f = x_t_1;
+            *y_t_f = y_t_1;
+
+        }
+    }
+    else
+    {
+        x_o = x_o_2;
+        y_o = y_o_2;
+        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
+        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
+
+        if (s1 < s2)
+        {
+
+            *x_t_n = x_t_3;
+            *y_t_n = y_t_3;
+            *x_t_f = x_t_4;
+            *y_t_f = y_t_4;
+        }
+        else
+        {
+            *x_t_n = x_t_4;
+            *y_t_n = y_t_4;
+            *x_t_f = x_t_3;
+            *y_t_f = y_t_3;
+        }
+
+
+
+    }
+
+    return 0;
+
+}
+
+
+int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
+{
+    int index = -1;
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+    float minDis=20;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis=tmpdis;
+        }
+    }
+    return index;
+}
+
+
+double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D>& gpsTrace, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {
+    double obsSpeed = 0 - realSecSpeed;
+    double minDis = iv::MaxValue;
+    FrenetPoint esr_obs_F_point;
+    for (int i = 0; i < 64; i++)
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
+        {
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
+
+            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+            {
+                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+                if (tmpDis < minDis)
+                {
+                    minDis = tmpDis;
+//                    esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
+                    esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
+//                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
+                    double speedx=ServiceCarStatus.obs_radar[i].speed_x;  //障碍物相对于车辆x轴的速度
+                    double speedy=ServiceCarStatus.obs_radar[i].speed_y;  //障碍物相对于车辆y轴的速度
+                    double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
+                    //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
+                    //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
+                    double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);
+
+                    obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
+                }
+            }
+        }
+
+    return obsSpeed;
+}
+
+
+int iv::decition::Compute00::getEsrIndexByFrenet(const std::vector<Point2D>& gpsTrace, FrenetPoint &esrObsPoint, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps){
+    double minDistance = numeric_limits<double>::max();
+    int minDis_index=-1;
+
+    for(int i=0; i<64; ++i){
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
+            //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+            //将毫米波障碍物位置转换到frenet坐标系下
+//            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
+            esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
+
+            //如果障碍物与道路的横向距离d<=3.0*Veh_Width / 4.0，则认为道路上有障碍物。
+            //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
+            //minDistance、minDis_index用来统计最近的障碍物信息。
+            if(abs(esrObsPoint.d)<=(3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs)){
+                if(esrObsPoint.s<minDistance){
+                    minDistance = esrObsPoint.s;
+                    minDis_index = i;
+                }
+            }
+        }
+    }
+    return minDis_index;
+}
+
+
+
+
+std::vector<std::vector<iv::GPSData>> gmapsL;
+std::vector<std::vector<iv::GPSData>> gmapsR;

src/decition/decition_brain_sf/decition/compute_00.h

@@ -1,96 +1,96 @@
-#pragma once
-#ifndef _IV_DECITION_COMPUTE_00_
-#define _IV_DECITION_COMPUTE_00_
-
-#include <common/gps_type.h>
-#include <common/obstacle_type.h>
-#include <decition/decition_type.h>
-#include<vector> 
-#include <decition/decide_gps_00.h>
-
-namespace iv {
-    namespace decition {
-    //根据传感器传输来的信息作出决策
-        class Compute00 {
-        public:
-            Compute00();
-            ~Compute00();
-
-            static   double maxAngle;
-            static	 double angleLimit;  //角度限制
-            static	 double lastEA;      //上一次角度误差
-            static	 double lastEP;      //上一次位置误差
-            static	 double decision_Angle;  //决策角度
-            static	 double lastAng;         //上次角度
-            static   int lastEsrID;          //上次毫米波障碍物ID
-            static   int  lastEsrCount;      //毫米波障碍物累计次数
-
-            static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
-            static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid
-
-            static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
-            static double bocheAngle,dBocheAngle;
-
-            static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
-            static	int getFirstNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
-
-            static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
-            static double getAveDef(std::vector<Point2D> farTrace);
-            static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
-            static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
-            static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
-
-
-            static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
-            static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint);
-            static int getEsrIndexAvoid(std::vector<Point2D> gpsTrace);
-
-            static double getObsSpeed(Point2D obsPoint, double realSecSpeed);
-
-            static double getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX);
-            static double getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX);
-
-            static std::vector<GPSData>  getBesideGpsMapLine(iv::GPS_INS now_gps_ins, std::vector<iv::GPSData>gpsMapLine, float avoidX);
-
-            static double getDecideAngleByLane(double realSpeed);
-
-            static double getDecideAngleByLanePID(double realSpeed);
-
-            static double bocheCompute(GPS_INS nowGps, GPS_INS aimGps);
-
-            static	double pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1);
-
-            static double getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps);
-
-            static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);
-
-            static double min_rad_zhuanxiang(double *R_M, double *min_rad);
-            static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
-                                    double *x_2, double *y_2, double *real_rad);
-            static double liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,
-                                                double ange1,double real_rad,double *x_3, double *y_3);
-            static double chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,
-                                                   double min_rad,double *angle_3);
-            static double yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,
-                                  double angle_3,double R_M,double *x_o_1, double *y_o_1, double *x_o_2, double *y_0_2);
-            static double yuanxin_qiedian(double angel,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
-                                          double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,
-                                          double real_rad,double angle_3,double R_M,
-                                          double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f);
-
-            static int getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap);
-    		static int getEsrIndexByFrenet(const std::vector<Point2D>& gpsTrace,FrenetPoint &esrObsPoint, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
-    		double getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const std::vector<Point2D>& gpsTrace, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
-        private:
-
-        };
-    }
-}
-extern std::vector<std::vector<iv::GPSData>> gmapsL;
-extern std::vector<std::vector<iv::GPSData>> gmapsR;
-
-extern std::vector<int> lastEsrIdVector;
-extern std::vector<int> lastEsrCountVector;
-
-#endif // !_IV_DECITION_COMPUTE_00_
-
+#pragma once
+#ifndef _IV_DECITION_COMPUTE_00_
+#define _IV_DECITION_COMPUTE_00_
+
+#include <common/gps_type.h>
+#include <common/obstacle_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+#include <decition/decide_gps_00.h>
+
+namespace iv {
+    namespace decition {
+    //根据传感器传输来的信息作出决策
+        class Compute00 {
+        public:
+            Compute00();
+            ~Compute00();
+
+            static   double maxAngle;
+            static	 double angleLimit;  //角度限制
+            static	 double lastEA;      //上一次角度误差
+            static	 double lastEP;      //上一次位置误差
+            static	 double decision_Angle;  //决策角度
+            static	 double lastAng;         //上次角度
+            static   int lastEsrID;          //上次毫米波障碍物ID
+            static   int  lastEsrCount;      //毫米波障碍物累计次数
+
+            static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
+            static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid
+
+            static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
+            static double bocheAngle,dBocheAngle;
+
+            static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
+            static	int getFirstNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
+
+            static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
+            static double getAveDef(std::vector<Point2D> farTrace);
+            static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
+            static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+
+
+            static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint);
+            static int getEsrIndexAvoid(std::vector<Point2D> gpsTrace);
+
+            static double getObsSpeed(Point2D obsPoint, double realSecSpeed);
+
+            static double getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX);
+            static double getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX);
+
+            static std::vector<GPSData>  getBesideGpsMapLine(iv::GPS_INS now_gps_ins, std::vector<iv::GPSData>gpsMapLine, float avoidX);
+
+            static double getDecideAngleByLane(double realSpeed);
+
+            static double getDecideAngleByLanePID(double realSpeed);
+
+            static double bocheCompute(GPS_INS nowGps, GPS_INS aimGps);
+
+            static	double pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1);
+
+            static double getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps);
+
+            static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);
+
+            static double min_rad_zhuanxiang(double *R_M, double *min_rad);
+            static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
+                                    double *x_2, double *y_2, double *real_rad);
+            static double liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,
+                                                double ange1,double real_rad,double *x_3, double *y_3);
+            static double chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,
+                                                   double min_rad,double *angle_3);
+            static double yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,
+                                  double angle_3,double R_M,double *x_o_1, double *y_o_1, double *x_o_2, double *y_0_2);
+            static double yuanxin_qiedian(double angel,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
+                                          double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,
+                                          double real_rad,double angle_3,double R_M,
+                                          double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f);
+
+            static int getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap);
+    		static int getEsrIndexByFrenet(const std::vector<Point2D>& gpsTrace,FrenetPoint &esrObsPoint, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+    		double getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const std::vector<Point2D>& gpsTrace, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+        private:
+
+        };
+    }
+}
+extern std::vector<std::vector<iv::GPSData>> gmapsL;
+extern std::vector<std::vector<iv::GPSData>> gmapsR;
+
+extern std::vector<int> lastEsrIdVector;
+extern std::vector<int> lastEsrCountVector;
+
+#endif // !_IV_DECITION_COMPUTE_00_
+

src/decition/decition_brain_sf/decition/decide_from_gps.cpp

@@ -1,605 +1,605 @@
-#include <decition/decition_maker.h>
-#include <decition/gps_distance.h>
-#include <decition/decition_type.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-using namespace std;
-
-
-iv::decition::DecitionMaker::DecitionMaker() {
-
-}
-iv::decition::DecitionMaker::~DecitionMaker() {
-
-}
-
-
-
-
-
-
-/* void iv::decition::DecitionMaker::decideFromGPS(Decition & gps_decition, iv::GPSData gps_data, const std::vector<iv::GPSData> navigation_data)
-{
-	//可直接获取车速  方法咨询张庆余
-
-	//样例
-	//输入数据类型
-	gps_data->gps_x;
-	gps_data->gps_y;
-	gps_data->gps_z;
-	gps_data->ins_heading_angle;	//航向角
-	gps_data->ins_pitch_angle;		//俯仰角
-	gps_data->ins_roll_angle;		//横滚角
-
-									//导航数据
-	navigation_data[0]->gps_x;
-	navigation_data[1]->gps_y;
-	for (std::vector<iv::GPSData>::const_iterator it = navigation_data.cbegin(); it != navigation_data.cend(); it++) {
-		//std::cout << (*it)->gps_x << std::endl;
-	}
-
-	//输出
-	gps_decition->speed = 30;
-	gps_decition->wheel_angle = 10;
-
-}*/
-
-
-void iv::decition::DecitionMaker::adjustOriginPoint(iv::GPS_INS nowGPSIns) {
-	const double PI = 3.1415926535898;
-	if (nowGPSIns.ins_heading_angle >= 0 && nowGPSIns.ins_heading_angle <= 90) {
-		origin_x = nowGPSIns.gps_x + vehLenthAdj*sin(nowGPSIns.ins_heading_angle*PI / 180);
-		origin_y = nowGPSIns.gps_y + vehLenthAdj*cos(nowGPSIns.ins_heading_angle*PI / 180);
-	}
-	else if (nowGPSIns.ins_heading_angle >= 90 && nowGPSIns.ins_heading_angle <= 180)
-	{
-		origin_x = nowGPSIns.gps_x + vehLenthAdj*sin((180 - nowGPSIns.ins_heading_angle)*PI / 180);
-		origin_y = nowGPSIns.gps_y - vehLenthAdj*cos((180 - nowGPSIns.ins_heading_angle)*PI / 180);
-	}
-	else  if (nowGPSIns.ins_heading_angle >= 180 && nowGPSIns.ins_heading_angle <= 270)
-	{
-		origin_x = nowGPSIns.gps_x - vehLenthAdj*sin((nowGPSIns.ins_heading_angle - 180)*PI / 180);
-		origin_y = nowGPSIns.gps_y - vehLenthAdj*cos((nowGPSIns.ins_heading_angle - 180)*PI / 180);
-	}
-	else if (nowGPSIns.ins_heading_angle >= 270 && nowGPSIns.ins_heading_angle <= 360)
-	{
-		origin_x = nowGPSIns.gps_x - vehLenthAdj*sin((360 - nowGPSIns.ins_heading_angle)*PI / 180);
-		origin_y = nowGPSIns.gps_y + vehLenthAdj*cos((360 - nowGPSIns.ins_heading_angle)*PI / 180);
-	}
-
-}
-
-
-
-
-/*
-	开启自动驾驶模式时，先获取离汽车距离最近的路径点
-*/
-
- void iv::decition::DecitionMaker::getStartPoint(iv::GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data) {
-	 double minDistance,distance;
-	 for (int i = lastNearPointNum; i < navigation_data.size(); i++)
-	 {
-		 distance = DirectDistance(origin_x, origin_y,(navigation_data[i])->gps_x, (navigation_data[i])->gps_y);
-//		 distance = CalcDistance(gps_ins.gps_lat, gps_ins.gps_lng, (navigation_data[i])->gps_lat, (navigation_data[i])->gps_lng);
-		 if (i==0)
-		 {
-			 minDistance = distance;
-		 }
-		 else {
-			 if (distance<minDistance)
-			 {
-				 minDistance = distance;
-				 lastNearPointNum = i;
-                 std::cout << "lastnearestpoint: %d\n" << lastNearPointNum << std::endl;
-//				 ODS("minDistance: %d\n", minDistance);
-			 }
-		 }
-	 }
-
-}
-
-
-
- /*
-     根据GPS坐标计算两点距离
- */
-
-double iv::decition::DecitionMaker::getDistanceDeviation(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns) {
-	return	CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nowGPSIns).gps_lat, (nowGPSIns).gps_lng);
-}
-
-
-/*
-    根据GPS坐标计算汽车与目标点之间的角度
-*/
-
-double iv::decition::DecitionMaker::getCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns) {
-	const double PI = 3.1415926535898;
-	double hypo = CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nowGPSIns).gps_lat, (nowGPSIns).gps_lng);
-	double hor = CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nearestGPSIns).gps_lat, (nowGPSIns).gps_lng);
-	double ang = asin(hor / hypo) / PI * 180;
-	double guideAng;
-	double crossAng; //顺时针是正直，逆时针是负值
-	if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat >= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng >= 0)) {
-		guideAng = ang;
-		//ang = 90 - ang;
-	}
-	else if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat >= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng <= 0))
-	{
-		guideAng = 360 - ang;
-		//ang = ang + 90;
-		
-	}
-	else if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat <= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng >= 0))
-	{
-		guideAng = 180 - ang;
-		//ang = ang - 90;
-	}
-	else  if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat <= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng <= 0))
-	{
-		guideAng = 180 + ang;
-		//ang = -90 - ang;
-	}
-
-	crossAng = guideAng - nowGPSIns.ins_heading_angle;
-	if (crossAng<-180)
-	{
-		crossAng = crossAng + 360;
-	}
-	else if (crossAng > 180) {
-		crossAng = crossAng - 360;
-	}
-	
-	
-	return crossAng;
-
-}
-
-/*
-	根据直角坐标计算汽车与目标点之间的角度
-*/
-
-double iv::decition::DecitionMaker::getDirectCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS objGPSIns) {
-
-	//double ang = DirectAngle(nowGPSIns.gps_x, nowGPSIns.gps_y,objGPSIns.gps_x,objGPSIns.gps_y);
-	double ang = DirectAngle(origin_x, origin_y, objGPSIns.gps_x, objGPSIns.gps_y);
-	double guideAng; //追随目标所需航向角
-	double crossAng; //顺时针是正直，逆时针是负值
-	if (((objGPSIns).gps_y - origin_y >= 0) && ((objGPSIns).gps_x - origin_x >= 0)) {
-		guideAng = 90 - ang;
-		//ang = 90 - ang;
-	}
-	else if (((objGPSIns).gps_y - origin_y >= 0) && ((objGPSIns).gps_x - origin_x <= 0))
-	{
-		guideAng = 270 - ang;
-		//ang = ang + 90;
-
-	}
-	else if (((objGPSIns).gps_y - origin_y <= 0) && ((objGPSIns).gps_x - origin_x >= 0))
-	{
-		guideAng = 90 - ang;
-		//ang = ang - 90;
-	}
-	else  if (((objGPSIns).gps_y - origin_y <= 0) && ((objGPSIns).gps_x - origin_x <= 0))
-	{
-		guideAng = 270 - ang;
-		//ang = -90 - ang;
-	}
-
-	crossAng = guideAng - nowGPSIns.ins_heading_angle;
-	if (crossAng<-180)
-	{
-		crossAng = crossAng + 360;
-	}
-	else if (crossAng > 180) {
-		crossAng = crossAng - 360;
-	}
-	return crossAng;
-}
-
-
-
-
-
-/*
-		根据GPS坐标搜索最近的车前路径点
-*/
-
-iv::GPS_INS iv::decition::DecitionMaker::getNearestGpsIns(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data) {
-	
-	GPS_INS lastNearPoint = *(navigation_data[lastNearPointNum]);
-
-	int count = navigation_data.size();
-
-	if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
-		for (int i = lastNearPointNum; i < count; i++) {
-			GPS_INS nearGNS = *(navigation_data[i]);
-
-			if ((nearGNS).gps_y>origin_y)
-			{
-				lastNearPointNum = i;
-				return nearGNS;
-			}
-		}
-	}
-	else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
-
-		for (int i = lastNearPointNum; i < count; i++)
-		{
-			GPS_INS nearGNS = *(navigation_data[i]);
-
-			if ((nearGNS).gps_x>origin_x)
-			{
-				lastNearPointNum = i;
-				return nearGNS;
-			}
-		}
-
-	}
-	else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
-
-		for (int i = lastNearPointNum; i < count; i++)
-		{
-			GPS_INS nearGNS = *(navigation_data[i]);
-
-			if ((nearGNS).gps_y<origin_y)
-			{
-				lastNearPointNum = i;
-				return nearGNS;
-			}
-		}
-
-	}
-	else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
-
-		for (int i = lastNearPointNum; i < count; i++)
-		{
-			GPS_INS nearGNS = *(navigation_data[i]);
-
-			if ((nearGNS).gps_x<origin_x)
-			{
-				lastNearPointNum = i;
-				return nearGNS;
-			}
-		}
-
-	}
-
-	return lastNearPoint;
-}
-
-
-/*
-  判断是否到达终点
-*/
-
-bool iv::decition::DecitionMaker::checkComplete(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data) {
-	if (lastNearPointNum < navigation_data.size() - 1) {
-		return false;
-	}
-	
-	GPS_INS lastNearPoint = *(navigation_data[lastNearPointNum]);
-
-	if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
-		if (origin_y>=lastNearPoint.gps_y)
-		{
-			return true;
-		}
-	}
-	else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
-		if (origin_x >= lastNearPoint.gps_x)
-		{
-			return true;
-		}
-		
-
-	}
-	else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
-
-		if (origin_y <= lastNearPoint.gps_y)
-		{
-			return true;
-		}
-
-	}
-	else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
-
-		if (origin_x <= lastNearPoint.gps_x)
-		{
-			return true;
-		}
-
-	}
-
-	return false;
-
-}
-
-
-
-
-
-
-/*
-	计算输出车速
-*/
-
-float iv::decition::DecitionMaker::getSpeedObj(GPS_INS nowGPSIns, GPS_INS objGPSIns) {
-	float speed;
-	int speedMode = objGPSIns.speed_mode;
-	/*switch (speedMode)
-	{
-	case 0:
-		nearSpeed = 0;
-		break;
-	case 1:
-		nearSpeed = 0;
-		break;
-	case 2:
-		nearSpeed = 0;
-		break;
-	case 3:
-		nearSpeed = 0;
-		break;
-	case 4:
-		nearSpeed = 0;
-		break;
-	case 5:
-		nearSpeed = 0;
-		break;
-	case 6:
-		nearSpeed = 0;
-		break;
-	case 7:
-		nearSpeed = 0;
-		break;
-	case 8:
-		nearSpeed = 0;
-		break;
-	default:
-		break;
-	}*/
-
-	float objSpeed = objGPSIns.speed;
-	float nowSpeed = nowGPSIns.speed;
-	if (objSpeed >nowSpeed) {
-		speed = nowGPSIns.speed + 1;
-	}
-	else if (objSpeed<nowSpeed)
-	{
-		speed = nowGPSIns.speed - 1;
-	}
-	else
-	{
-		speed = objSpeed;
-	}
-	return speed;
-}
-
-/*
-   检验车与目标轨迹距离是否偏大
-*/
-
-void iv::decition::DecitionMaker::checkDistance(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data,double maxDistance) {
-	  double distanceF = DirectDistance(nowGPSIns.gps_x, nowGPSIns.gps_y, (navigation_data[lastNearPointNum])->gps_x, (navigation_data[lastNearPointNum])->gps_y);	
-	  if (distanceF>maxDistance)
-	  {
-        std::cout << "您已远离目标路线\n" << std::endl;
-	  }
-}
-
-
-string getTime()
-{
-	time_t timep;
-	time(&timep);
-	char tmp[64];
-	strftime(tmp, sizeof(tmp), "%Y-%m-%d %H:%M:%S", localtime(&timep));
-	return tmp;
-}
-
-/*
-	给出GPS决策Decition
-*/
-iv::decition::Decition iv::decition::DecitionMaker::getDecideForGPS(iv::GPS_INS now_gps_ins, const std::vector<iv::GPSData> navigation_data) {
-	Decition gps_decition(new DecitionBasic);
-	//DecitionBasic decitionBasic;
-	int aheadNum;
-	if (now_gps_ins.speed_mode>2)
-	{
-		aheadNum = 48;
-	}
-	else {
-		aheadNum = 48;
-	}
-	origin_x = now_gps_ins.gps_x;
-	origin_y = now_gps_ins.gps_y;
-	adjustOriginPoint(now_gps_ins);
-	
-	if (isFirstRun)
-	{
-		getStartPoint(now_gps_ins, navigation_data);
-		isFirstRun = false;
-	}
-	nearestGpsIns = getNearestGpsIns(now_gps_ins, navigation_data);
-    std::cout << "\nlast :%d\t%f\t%f \n" << lastNearPointNum << now_gps_ins.gps_lat << now_gps_ins.gps_lng << std::endl;
-    std::cout << "nearest index: %d\t%f\t%f\n" << nearestGpsIns.index << nearestGpsIns.gps_lat << nearestGpsIns.gps_lng <<std::endl;
-	if (!checkComplete(now_gps_ins, navigation_data))
-	{
-		objPointNum = lastNearPointNum+aheadNum;
-		if (objPointNum > navigation_data.size()-1)
-			objPointNum = aheadNum -1;
-		objGpsIns = *(navigation_data[objPointNum]);
-		double angle = getDirectCrossAngle(now_gps_ins, objGpsIns);
-
-	//	checkDistance(now_gps_ins, navigation_data, 5000);
-
-		gps_decition->speed = getSpeedObj(now_gps_ins, nearestGpsIns);
-		if (angle>5||angle<-5)
-		{
-			gps_decition->wheel_angle = angle;
-		}
-		else
-		{
-			gps_decition->wheel_angle = 0;
-		}
-		//string time = getTime();
-		//ODS("当前时间: %s\n", lastNearPointNum);
-		gps_decition->wheel_angle = 0 - gps_decition->wheel_angle*40;
-		if (gps_decition->wheel_angle > 4000) {
-			gps_decition->wheel_angle = 4000;
-		}
-		if (gps_decition->wheel_angle < -4000) {
-			gps_decition->wheel_angle = -4000;
-		}
-	}
-	else {
-		gps_decition->wheel_angle = 0;
-		gps_decition->speed = 0;
-	}
-	
-	return gps_decition;
-
-}
-
-
-//string iv::decition::DecitionMaker::getTime()
-// {
-//	     time_t timep;
-//	     time(&timep);
-//	     char tmp[64];
-//	     strftime(tmp, sizeof(tmp), "%Y-%m-%d %H:%M:%S", localtime(&timep));
-//	    return tmp;
-//}
-
-
-/*iv::GPS_INS iv::decition::DecitionMaker::getNearestGpsIns(iv::GPS_INS gps_ins) {
-
-GPS_INS lastNearPoint = vec_Gps_Group[lastNearPointNum];
-
-int count = vec_Gps_Group.size();
-
-if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
-for (int i = lastNearPointNum; i < count;i++) {
-GPS_INS objGD = vec_Gps_Group[i];
-
-if ((objGD).gps_lat>(gps_ins).gps_lat)
-{
-lastNearPointNum = i;
-return objGD;
-}
-}
-}
-else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
-
-for (int i = lastNearPointNum; i < count;i++)
-{
-GPS_INS objGD = vec_Gps_Group[i];
-
-if ((objGD).gps_lng>(gps_ins).gps_lng)
-{
-lastNearPointNum = i;
-return objGD;
-}
-}
-
-}
-else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
-
-for (int i = lastNearPointNum; i < count;i++)
-{
-GPS_INS objGD = vec_Gps_Group[i];
-
-if ((objGD).gps_lat<(gps_ins).gps_lat)
-{
-lastNearPointNum = i;
-return objGD;
-}
-}
-
-}
-else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
-
-for (int i = lastNearPointNum; i < count;i++)
-{
-GPS_INS objGD = vec_Gps_Group[i];
-
-if ((objGD).gps_lng<(gps_ins).gps_lng)
-{
-lastNearPointNum = i;
-return objGD;
-}
-}
-
-}
-
-return lastNearPoint;
-}*/
-
-
-
-/*iv::decition::Decition iv::decition::DecitionMaker::getDecideForGPS(iv::GPS_INS now_gps_ins) {
-	Decition gps_decition(new DecitionBasic);
-	//DecitionBasic decitionBasic;
-	nearestGpsIns = getNearestGpsIns(now_gps_ins);
-	double angle = getCrossAngle(now_gps_ins, nearestGpsIns);
-
-	gps_decition->speed = getSpeedObj(now_gps_ins, nearestGpsIns);
-	gps_decition->wheel_angle = angle;
-	return gps_decition;
-
-}*/
-
-
-/*void iv::decition::DecitionMaker::readFromText() {
-
-	ifstream infile;
-	infile.open("D:\\auto_car\\gps_trace.txt");
-	if (!infile) cout << "error" << endl;
-	string str;
-	int t1 = 0;
-	
-	
-	while (getline(infile, str))   //按行读取,遇到换行符结束
-	{
-
-		
-		cout << str << endl;
-		GPS_INS gps_Ins;
-
-		while (infile >> str)
-		{
-			// ODS("%s\n", str);
-			
-			cout << str << endl;
-			t1++;
-			if (t1 == 2)
-			{
-				gps_Ins.gps_lng = stod(str);
-			}
-			else if (t1 == 3)
-			{
-				gps_Ins.gps_lat = stod(str);
-			}
-			else if (t1 == 4)
-			{
-				gps_Ins.speed = stod(str);
-			}
-			else if (t1 == 6)
-			{
-				gps_Ins.ins_heading_angle = stod(str);
-				vec_Gps_Group.push_back(gps_Ins);
-				t1 = 0;
-			}
-		}
-		
-		
-	}
-	infile.close();
-}*/
-
-
-
-
+#include <decition/decition_maker.h>
+#include <decition/gps_distance.h>
+#include <decition/decition_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+using namespace std;
+
+
+iv::decition::DecitionMaker::DecitionMaker() {
+
+}
+iv::decition::DecitionMaker::~DecitionMaker() {
+
+}
+
+
+
+
+
+
+/* void iv::decition::DecitionMaker::decideFromGPS(Decition & gps_decition, iv::GPSData gps_data, const std::vector<iv::GPSData> navigation_data)
+{
+	//可直接获取车速  方法咨询张庆余
+
+	//样例
+	//输入数据类型
+	gps_data->gps_x;
+	gps_data->gps_y;
+	gps_data->gps_z;
+	gps_data->ins_heading_angle;	//航向角
+	gps_data->ins_pitch_angle;		//俯仰角
+	gps_data->ins_roll_angle;		//横滚角
+
+									//导航数据
+	navigation_data[0]->gps_x;
+	navigation_data[1]->gps_y;
+	for (std::vector<iv::GPSData>::const_iterator it = navigation_data.cbegin(); it != navigation_data.cend(); it++) {
+		//std::cout << (*it)->gps_x << std::endl;
+	}
+
+	//输出
+	gps_decition->speed = 30;
+	gps_decition->wheel_angle = 10;
+
+}*/
+
+
+void iv::decition::DecitionMaker::adjustOriginPoint(iv::GPS_INS nowGPSIns) {
+	const double PI = 3.1415926535898;
+	if (nowGPSIns.ins_heading_angle >= 0 && nowGPSIns.ins_heading_angle <= 90) {
+		origin_x = nowGPSIns.gps_x + vehLenthAdj*sin(nowGPSIns.ins_heading_angle*PI / 180);
+		origin_y = nowGPSIns.gps_y + vehLenthAdj*cos(nowGPSIns.ins_heading_angle*PI / 180);
+	}
+	else if (nowGPSIns.ins_heading_angle >= 90 && nowGPSIns.ins_heading_angle <= 180)
+	{
+		origin_x = nowGPSIns.gps_x + vehLenthAdj*sin((180 - nowGPSIns.ins_heading_angle)*PI / 180);
+		origin_y = nowGPSIns.gps_y - vehLenthAdj*cos((180 - nowGPSIns.ins_heading_angle)*PI / 180);
+	}
+	else  if (nowGPSIns.ins_heading_angle >= 180 && nowGPSIns.ins_heading_angle <= 270)
+	{
+		origin_x = nowGPSIns.gps_x - vehLenthAdj*sin((nowGPSIns.ins_heading_angle - 180)*PI / 180);
+		origin_y = nowGPSIns.gps_y - vehLenthAdj*cos((nowGPSIns.ins_heading_angle - 180)*PI / 180);
+	}
+	else if (nowGPSIns.ins_heading_angle >= 270 && nowGPSIns.ins_heading_angle <= 360)
+	{
+		origin_x = nowGPSIns.gps_x - vehLenthAdj*sin((360 - nowGPSIns.ins_heading_angle)*PI / 180);
+		origin_y = nowGPSIns.gps_y + vehLenthAdj*cos((360 - nowGPSIns.ins_heading_angle)*PI / 180);
+	}
+
+}
+
+
+
+
+/*
+	开启自动驾驶模式时，先获取离汽车距离最近的路径点
+*/
+
+ void iv::decition::DecitionMaker::getStartPoint(iv::GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data) {
+	 double minDistance,distance;
+	 for (int i = lastNearPointNum; i < navigation_data.size(); i++)
+	 {
+		 distance = DirectDistance(origin_x, origin_y,(navigation_data[i])->gps_x, (navigation_data[i])->gps_y);
+//		 distance = CalcDistance(gps_ins.gps_lat, gps_ins.gps_lng, (navigation_data[i])->gps_lat, (navigation_data[i])->gps_lng);
+		 if (i==0)
+		 {
+			 minDistance = distance;
+		 }
+		 else {
+			 if (distance<minDistance)
+			 {
+				 minDistance = distance;
+				 lastNearPointNum = i;
+                 std::cout << "lastnearestpoint: %d\n" << lastNearPointNum << std::endl;
+//				 ODS("minDistance: %d\n", minDistance);
+			 }
+		 }
+	 }
+
+}
+
+
+
+ /*
+     根据GPS坐标计算两点距离
+ */
+
+double iv::decition::DecitionMaker::getDistanceDeviation(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns) {
+	return	CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nowGPSIns).gps_lat, (nowGPSIns).gps_lng);
+}
+
+
+/*
+    根据GPS坐标计算汽车与目标点之间的角度
+*/
+
+double iv::decition::DecitionMaker::getCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns) {
+	const double PI = 3.1415926535898;
+	double hypo = CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nowGPSIns).gps_lat, (nowGPSIns).gps_lng);
+	double hor = CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nearestGPSIns).gps_lat, (nowGPSIns).gps_lng);
+	double ang = asin(hor / hypo) / PI * 180;
+	double guideAng;
+	double crossAng; //顺时针是正直，逆时针是负值
+	if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat >= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng >= 0)) {
+		guideAng = ang;
+		//ang = 90 - ang;
+	}
+	else if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat >= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng <= 0))
+	{
+		guideAng = 360 - ang;
+		//ang = ang + 90;
+		
+	}
+	else if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat <= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng >= 0))
+	{
+		guideAng = 180 - ang;
+		//ang = ang - 90;
+	}
+	else  if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat <= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng <= 0))
+	{
+		guideAng = 180 + ang;
+		//ang = -90 - ang;
+	}
+
+	crossAng = guideAng - nowGPSIns.ins_heading_angle;
+	if (crossAng<-180)
+	{
+		crossAng = crossAng + 360;
+	}
+	else if (crossAng > 180) {
+		crossAng = crossAng - 360;
+	}
+	
+	
+	return crossAng;
+
+}
+
+/*
+	根据直角坐标计算汽车与目标点之间的角度
+*/
+
+double iv::decition::DecitionMaker::getDirectCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS objGPSIns) {
+
+	//double ang = DirectAngle(nowGPSIns.gps_x, nowGPSIns.gps_y,objGPSIns.gps_x,objGPSIns.gps_y);
+	double ang = DirectAngle(origin_x, origin_y, objGPSIns.gps_x, objGPSIns.gps_y);
+	double guideAng; //追随目标所需航向角
+	double crossAng; //顺时针是正直，逆时针是负值
+	if (((objGPSIns).gps_y - origin_y >= 0) && ((objGPSIns).gps_x - origin_x >= 0)) {
+		guideAng = 90 - ang;
+		//ang = 90 - ang;
+	}
+	else if (((objGPSIns).gps_y - origin_y >= 0) && ((objGPSIns).gps_x - origin_x <= 0))
+	{
+		guideAng = 270 - ang;
+		//ang = ang + 90;
+
+	}
+	else if (((objGPSIns).gps_y - origin_y <= 0) && ((objGPSIns).gps_x - origin_x >= 0))
+	{
+		guideAng = 90 - ang;
+		//ang = ang - 90;
+	}
+	else  if (((objGPSIns).gps_y - origin_y <= 0) && ((objGPSIns).gps_x - origin_x <= 0))
+	{
+		guideAng = 270 - ang;
+		//ang = -90 - ang;
+	}
+
+	crossAng = guideAng - nowGPSIns.ins_heading_angle;
+	if (crossAng<-180)
+	{
+		crossAng = crossAng + 360;
+	}
+	else if (crossAng > 180) {
+		crossAng = crossAng - 360;
+	}
+	return crossAng;
+}
+
+
+
+
+
+/*
+		根据GPS坐标搜索最近的车前路径点
+*/
+
+iv::GPS_INS iv::decition::DecitionMaker::getNearestGpsIns(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data) {
+	
+	GPS_INS lastNearPoint = *(navigation_data[lastNearPointNum]);
+
+	int count = navigation_data.size();
+
+	if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
+		for (int i = lastNearPointNum; i < count; i++) {
+			GPS_INS nearGNS = *(navigation_data[i]);
+
+			if ((nearGNS).gps_y>origin_y)
+			{
+				lastNearPointNum = i;
+				return nearGNS;
+			}
+		}
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
+
+		for (int i = lastNearPointNum; i < count; i++)
+		{
+			GPS_INS nearGNS = *(navigation_data[i]);
+
+			if ((nearGNS).gps_x>origin_x)
+			{
+				lastNearPointNum = i;
+				return nearGNS;
+			}
+		}
+
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
+
+		for (int i = lastNearPointNum; i < count; i++)
+		{
+			GPS_INS nearGNS = *(navigation_data[i]);
+
+			if ((nearGNS).gps_y<origin_y)
+			{
+				lastNearPointNum = i;
+				return nearGNS;
+			}
+		}
+
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
+
+		for (int i = lastNearPointNum; i < count; i++)
+		{
+			GPS_INS nearGNS = *(navigation_data[i]);
+
+			if ((nearGNS).gps_x<origin_x)
+			{
+				lastNearPointNum = i;
+				return nearGNS;
+			}
+		}
+
+	}
+
+	return lastNearPoint;
+}
+
+
+/*
+  判断是否到达终点
+*/
+
+bool iv::decition::DecitionMaker::checkComplete(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data) {
+	if (lastNearPointNum < navigation_data.size() - 1) {
+		return false;
+	}
+	
+	GPS_INS lastNearPoint = *(navigation_data[lastNearPointNum]);
+
+	if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
+		if (origin_y>=lastNearPoint.gps_y)
+		{
+			return true;
+		}
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
+		if (origin_x >= lastNearPoint.gps_x)
+		{
+			return true;
+		}
+		
+
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
+
+		if (origin_y <= lastNearPoint.gps_y)
+		{
+			return true;
+		}
+
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
+
+		if (origin_x <= lastNearPoint.gps_x)
+		{
+			return true;
+		}
+
+	}
+
+	return false;
+
+}
+
+
+
+
+
+
+/*
+	计算输出车速
+*/
+
+float iv::decition::DecitionMaker::getSpeedObj(GPS_INS nowGPSIns, GPS_INS objGPSIns) {
+	float speed;
+	int speedMode = objGPSIns.speed_mode;
+	/*switch (speedMode)
+	{
+	case 0:
+		nearSpeed = 0;
+		break;
+	case 1:
+		nearSpeed = 0;
+		break;
+	case 2:
+		nearSpeed = 0;
+		break;
+	case 3:
+		nearSpeed = 0;
+		break;
+	case 4:
+		nearSpeed = 0;
+		break;
+	case 5:
+		nearSpeed = 0;
+		break;
+	case 6:
+		nearSpeed = 0;
+		break;
+	case 7:
+		nearSpeed = 0;
+		break;
+	case 8:
+		nearSpeed = 0;
+		break;
+	default:
+		break;
+	}*/
+
+	float objSpeed = objGPSIns.speed;
+	float nowSpeed = nowGPSIns.speed;
+	if (objSpeed >nowSpeed) {
+		speed = nowGPSIns.speed + 1;
+	}
+	else if (objSpeed<nowSpeed)
+	{
+		speed = nowGPSIns.speed - 1;
+	}
+	else
+	{
+		speed = objSpeed;
+	}
+	return speed;
+}
+
+/*
+   检验车与目标轨迹距离是否偏大
+*/
+
+void iv::decition::DecitionMaker::checkDistance(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data,double maxDistance) {
+	  double distanceF = DirectDistance(nowGPSIns.gps_x, nowGPSIns.gps_y, (navigation_data[lastNearPointNum])->gps_x, (navigation_data[lastNearPointNum])->gps_y);	
+	  if (distanceF>maxDistance)
+	  {
+        std::cout << "您已远离目标路线\n" << std::endl;
+	  }
+}
+
+
+string getTime()
+{
+	time_t timep;
+	time(&timep);
+	char tmp[64];
+	strftime(tmp, sizeof(tmp), "%Y-%m-%d %H:%M:%S", localtime(&timep));
+	return tmp;
+}
+
+/*
+	给出GPS决策Decition
+*/
+iv::decition::Decition iv::decition::DecitionMaker::getDecideForGPS(iv::GPS_INS now_gps_ins, const std::vector<iv::GPSData> navigation_data) {
+	Decition gps_decition(new DecitionBasic);
+	//DecitionBasic decitionBasic;
+	int aheadNum;
+	if (now_gps_ins.speed_mode>2)
+	{
+		aheadNum = 48;
+	}
+	else {
+		aheadNum = 48;
+	}
+	origin_x = now_gps_ins.gps_x;
+	origin_y = now_gps_ins.gps_y;
+	adjustOriginPoint(now_gps_ins);
+	
+	if (isFirstRun)
+	{
+		getStartPoint(now_gps_ins, navigation_data);
+		isFirstRun = false;
+	}
+	nearestGpsIns = getNearestGpsIns(now_gps_ins, navigation_data);
+    std::cout << "\nlast :%d\t%f\t%f \n" << lastNearPointNum << now_gps_ins.gps_lat << now_gps_ins.gps_lng << std::endl;
+    std::cout << "nearest index: %d\t%f\t%f\n" << nearestGpsIns.index << nearestGpsIns.gps_lat << nearestGpsIns.gps_lng <<std::endl;
+	if (!checkComplete(now_gps_ins, navigation_data))
+	{
+		objPointNum = lastNearPointNum+aheadNum;
+		if (objPointNum > navigation_data.size()-1)
+			objPointNum = aheadNum -1;
+		objGpsIns = *(navigation_data[objPointNum]);
+		double angle = getDirectCrossAngle(now_gps_ins, objGpsIns);
+
+	//	checkDistance(now_gps_ins, navigation_data, 5000);
+
+		gps_decition->speed = getSpeedObj(now_gps_ins, nearestGpsIns);
+		if (angle>5||angle<-5)
+		{
+			gps_decition->wheel_angle = angle;
+		}
+		else
+		{
+			gps_decition->wheel_angle = 0;
+		}
+		//string time = getTime();
+		//ODS("当前时间: %s\n", lastNearPointNum);
+		gps_decition->wheel_angle = 0 - gps_decition->wheel_angle*40;
+		if (gps_decition->wheel_angle > 4000) {
+			gps_decition->wheel_angle = 4000;
+		}
+		if (gps_decition->wheel_angle < -4000) {
+			gps_decition->wheel_angle = -4000;
+		}
+	}
+	else {
+		gps_decition->wheel_angle = 0;
+		gps_decition->speed = 0;
+	}
+	
+	return gps_decition;
+
+}
+
+
+//string iv::decition::DecitionMaker::getTime()
+// {
+//	     time_t timep;
+//	     time(&timep);
+//	     char tmp[64];
+//	     strftime(tmp, sizeof(tmp), "%Y-%m-%d %H:%M:%S", localtime(&timep));
+//	    return tmp;
+//}
+
+
+/*iv::GPS_INS iv::decition::DecitionMaker::getNearestGpsIns(iv::GPS_INS gps_ins) {
+
+GPS_INS lastNearPoint = vec_Gps_Group[lastNearPointNum];
+
+int count = vec_Gps_Group.size();
+
+if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
+for (int i = lastNearPointNum; i < count;i++) {
+GPS_INS objGD = vec_Gps_Group[i];
+
+if ((objGD).gps_lat>(gps_ins).gps_lat)
+{
+lastNearPointNum = i;
+return objGD;
+}
+}
+}
+else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
+
+for (int i = lastNearPointNum; i < count;i++)
+{
+GPS_INS objGD = vec_Gps_Group[i];
+
+if ((objGD).gps_lng>(gps_ins).gps_lng)
+{
+lastNearPointNum = i;
+return objGD;
+}
+}
+
+}
+else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
+
+for (int i = lastNearPointNum; i < count;i++)
+{
+GPS_INS objGD = vec_Gps_Group[i];
+
+if ((objGD).gps_lat<(gps_ins).gps_lat)
+{
+lastNearPointNum = i;
+return objGD;
+}
+}
+
+}
+else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
+
+for (int i = lastNearPointNum; i < count;i++)
+{
+GPS_INS objGD = vec_Gps_Group[i];
+
+if ((objGD).gps_lng<(gps_ins).gps_lng)
+{
+lastNearPointNum = i;
+return objGD;
+}
+}
+
+}
+
+return lastNearPoint;
+}*/
+
+
+
+/*iv::decition::Decition iv::decition::DecitionMaker::getDecideForGPS(iv::GPS_INS now_gps_ins) {
+	Decition gps_decition(new DecitionBasic);
+	//DecitionBasic decitionBasic;
+	nearestGpsIns = getNearestGpsIns(now_gps_ins);
+	double angle = getCrossAngle(now_gps_ins, nearestGpsIns);
+
+	gps_decition->speed = getSpeedObj(now_gps_ins, nearestGpsIns);
+	gps_decition->wheel_angle = angle;
+	return gps_decition;
+
+}*/
+
+
+/*void iv::decition::DecitionMaker::readFromText() {
+
+	ifstream infile;
+	infile.open("D:\\auto_car\\gps_trace.txt");
+	if (!infile) cout << "error" << endl;
+	string str;
+	int t1 = 0;
+	
+	
+	while (getline(infile, str))   //按行读取,遇到换行符结束
+	{
+
+		
+		cout << str << endl;
+		GPS_INS gps_Ins;
+
+		while (infile >> str)
+		{
+			// ODS("%s\n", str);
+			
+			cout << str << endl;
+			t1++;
+			if (t1 == 2)
+			{
+				gps_Ins.gps_lng = stod(str);
+			}
+			else if (t1 == 3)
+			{
+				gps_Ins.gps_lat = stod(str);
+			}
+			else if (t1 == 4)
+			{
+				gps_Ins.speed = stod(str);
+			}
+			else if (t1 == 6)
+			{
+				gps_Ins.ins_heading_angle = stod(str);
+				vec_Gps_Group.push_back(gps_Ins);
+				t1 = 0;
+			}
+		}
+		
+		
+	}
+	infile.close();
+}*/
+
+
+
+

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1,3375 +1,3375 @@
-#include <decition/decide_gps_00.h>
-#include <decition/adc_tools/compute_00.h>
-#include <decition/adc_tools/gps_distance.h>
-#include <decition/decition_type.h>
-#include <decition/adc_tools/transfer.h>
-#include <decition/obs_predict.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-#include <control/can.h>
-#include <time.h>
-#include <decition/adc_controller/base_controller.h>
-#include <decition/adc_controller/pid_controller.h>
-#include <decition/adc_planner/lane_change_planner.h>
-#include <decition/adc_planner/frenet_planner.h>
-#include <QTime>
-
-using namespace std;
-
-
-#include "ivlog.h"
-extern iv::Ivlog * givlog;
-
-#define PI (3.1415926535897932384626433832795)
-
-iv::decition::DecideGps00::DecideGps00() {
-
-}
-iv::decition::DecideGps00::~DecideGps00() {
-
-}
-
-float iv::decition::DecideGps00::xiuzhengCs=0;
-
-int iv::decition::DecideGps00::PathPoint = -1;
-double iv::decition::DecideGps00::minDis = iv::MaxValue;
-double iv::decition::DecideGps00::maxAngle = iv::MinValue;
-//int iv::decition::DecideGps00::lastGpsIndex = iv::MaxValue;
-int iv::decition::DecideGps00::lastGpsIndex = 0;
-double iv::decition::DecideGps00::controlSpeed = 0;
-double iv::decition::DecideGps00::controlBrake = 0;
-double iv::decition::DecideGps00::controlAng = 0;
-double iv::decition::DecideGps00::Iv = 0;
-double iv::decition::DecideGps00::lastU = 0;
-double iv::decition::DecideGps00::lastVt = 0;
-double iv::decition::DecideGps00::lastEv = 0;
-
-int iv::decition::DecideGps00::gpsLineParkIndex = 0;
-int iv::decition::DecideGps00::gpsMapParkIndex = 0;
-double iv::decition::DecideGps00::lastDistance = MaxValue;
-double iv::decition::DecideGps00::lastPreObsDistance = MaxValue;
-double iv::decition::DecideGps00::obsDistance = -1;
-double iv::decition::DecideGps00::obsSpeed=0;
-double iv::decition::DecideGps00::obsDistanceAvoid = -1;
-double iv::decition::DecideGps00::lastDistanceAvoid = -1;
-
-double iv::decition::DecideGps00::lidarDistance = -1;
-double iv::decition::DecideGps00::myesrDistance = -1;
-double iv::decition::DecideGps00::lastLidarDis = -1;
-
-bool iv::decition::DecideGps00::parkMode = false;
-bool iv::decition::DecideGps00::readyParkMode = false;
-
-bool iv::decition::DecideGps00::zhucheMode = false;
-bool iv::decition::DecideGps00::readyZhucheMode = false;
-bool iv::decition::DecideGps00::hasZhuched = false;
-
-double iv::decition::DecideGps00::lastLidarDisAvoid = -1;
-double iv::decition::DecideGps00::lidarDistanceAvoid = -1;
-
-int iv::decition::DecideGps00::finishPointNum = -1;
-int iv::decition::DecideGps00::zhuchePointNum = 0;
-
-///kkcai, 2020-01-03
-bool iv::decition::DecideGps00::isFirstRun = true;
-//////////////////////////////////////////////
-
-float iv::decition::DecideGps00::minDecelerate=0;
-//bool iv::decition::DecideGps00::startingFlag = true;//起步标志，在起步时需要进行frenet规划。
-
-double offset_real = 0;
-double realspeed;
-double dSpeed;
-double dSecSpeed;
-double secSpeed;
-double ac;
-
-
-iv::Point2D obsPoint(-1, -1);
-iv::Point2D obsPointAvoid(-1, -1);
-
-
-int esrIndex = -1;
-int esrIndexAvoid = -1;
-double obsSpeedAvoid = 0;
-
-double esrDistance = -1;
-double esrDistanceAvoid = -1;
-int obsLostTime = 0;
-int obsLostTimeAvoid = 0;
-
-// double avoidTime = 0;
-
-
-double avoidX =0;
-float roadWidth = 3.5;
-int roadSum =10;
-int roadNow = 0;
-int roadOri =0;
-int roadPre = -1;
-int lastRoadOri = 0;
-
-int lightTimes = 0;
-
-
-int lastRoadNum=1;
-
-int stopCount = 0;
-
-int gpsMissCount = 0;
-
-bool rapidStop = false;
-
-bool hasTrumpeted = false;
-
-
-bool changeRoad=false;
-//实验手刹
-bool handFirst = true;
-double handTimeSpan = 0;
-double handStartTime = 0;
-double handLastTime = 0;
-bool handPark = false;
-long handParkTime=10000;
-
-//喇叭
-bool trumpetFirstCount=true;
-double trumpetTimeSpan = 0;
-double trumpetStartTime = 0;
-double trumpetLastTime = 0;
-
-//过渡
-bool transferFirstCount=true;
-double transferTimeSpan = 0;
-double transferStartTime = 0;
-double transferLastTime = 0;
-
-bool busMode = false;
-bool parkBesideRoad=false;
-
-bool chaocheMode = false;
-bool specialSignle = false;
-
-double offsetX = 0;
-
-double steerSpeed=9000;
-
-bool transferPieriod;
-bool transferPieriod2;
-double traceDevi;
-
-bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划。
-bool useFrenet = false;    //标志是否启用frenet算法避障
-bool useOldAvoid = true;   //标志是否用老方法避障
-
-enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
-                waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
-                dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState;
-
-
-std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
-std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear;
-
-std::vector<double> esrDisVector(roadSum, -1);
-std::vector<double> lidarDisVector(roadSum, -1);
-std::vector<double> obsDisVector(roadSum,-1);
-std::vector<double> obsSpeedVector(roadSum, 0);
-
-std::vector<int> obsLostTimeVector(roadSum, 0);
-
-std::vector<double> lastLidarDisVector(roadSum, -1);
-std::vector<double> lastDistanceVector(roadSum, -1);
-
-bool qiedianCount = false;
-//日常展示
-
-iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
-                                                                   const std::vector<GPSData> gpsMapLine,
-                                                                   iv::LidarGridPtr lidarGridPtr,
-                                                                   std::vector<iv::Perception::PerceptionOutput> lidar_per,
-                                                                   iv::TrafficLight trafficLight)
-{
-    //   QTime xTime;
-    //    xTime.start();
-    //39.14034649083606 117.0863975920476 20507469.630314853  4334165.046101382 353
-    Decition gps_decition(new DecitionBasic);
-    //    vector<iv::Point2D> fpTraceTmp;
-
-
-    //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
-    if(!(useFrenet^useOldAvoid)){
-        useFrenet = false;
-        useOldAvoid = true;
-    }
-
-    //    //如果useFrenet、useOldAvoid两者均为真或均为假，则采用原来的方法
-    //    if(useFrenet&&useOldAvoid || !useFrenet&&!useOldAvoid){
-    //        useFrenet = false;
-    //        useOldAvoid = true;
-    //    }
-
-    //  ServiceCarStatus.group_control=false;
-
-
-
-
-//    GPS_INS gps= Coordinate_UnTransfer(0,1.5,now_gps_ins);
-//    now_gps_ins.gps_x=gps.gps_x;
-//    now_gps_ins.gps_y=gps.gps_y;
-
-    //    GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
-
-
-    if (isFirstRun)
-    {
-        initMethods();
-        vehState = normalRun;
-        startAvoid_gps_ins = now_gps_ins;
-        init();
-        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
-                                                          DecideGps00::lastGpsIndex,
-                                                          DecideGps00::minDis,
-                                                          DecideGps00::maxAngle);
-        DecideGps00::lastGpsIndex = PathPoint;
-
-        adjuseGpsLidarPosition();
-        if(ServiceCarStatus.speed_control==true){
-            Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
-        }
-
-        //	ServiceCarStatus.carState = 1;
-        //        roadOri = gpsMapLine[PathPoint]->roadOri;
-        //        roadNow = roadOri;
-        //        roadSum = gpsMapLine[PathPoint]->roadSum;
-        //  busMode = false;
-        //  vehState = dRever;
-
-        double dis =  GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
-        if(dis<15){
-            circleMode=true;  //201200102
-        }else{
-            circleMode=false;
-        }
-        //     circleMode = true;
-
-
-        getV2XTrafficPositionVector(gpsMapLine);
-        //        group_ori_gps=*gpsMapLine[0];
-        ServiceCarStatus.bocheMode=false;
-        ServiceCarStatus.daocheMode=false;
-        parkMode=false;
-        readyParkMode=false;
-        finishPointNum=-1;
-        isFirstRun = false;
-    }
-
-
-    if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
-        GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
-        now_gps_ins.gps_x=gpsOffset.gps_x;
-        now_gps_ins.gps_y=gpsOffset.gps_y;
-        GaussProjInvCal(now_gps_ins.gps_x,  now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
-    }
-
-
-
-    //1227
-    //  ServiceCarStatus.daocheMode=true;
-
-    //1220
-    changingDangWei=false;
-
-    minDecelerate=0;
-    if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90){
-        //  int a=0;
-        gps_decition->wheel_angle = 0;
-        gps_decition->speed = dSpeed;
-
-        gps_decition->accelerator = -0.5;
-        gps_decition->brake=10;
-        return gps_decition;
-    }
-
-
-
-
-
-    //1220
-
-
-    if(ServiceCarStatus.daocheMode){
-
-        now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
-        if( now_gps_ins.ins_heading_angle>=360)
-            now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
-    }
-    //1220 end
-
-
-
-    //1125 traficc
-    traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
-    traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
-    GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
-
-
-    //xiesi
-    ///kkcai, 2020-01-03
-    //ServiceCarStatus.busmode=true;
-    //ServiceCarStatus.busmode=false;//20200102
-    ///////////////////////////////////////////////////
-
-
-    busMode = ServiceCarStatus.busmode;
-    nearStation=false;
-
-    gps_decition->leftlamp = false;
-    gps_decition->rightlamp = false;
-    //    qDebug("heading is %g",now_gps_ins.ins_heading_angle);
-
-
-    aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
-    aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;
-
-    aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;
-
-
-
-
-    GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-    is_arrivaled = false;
-
-
-
-
-    //  xiuzhengCs=-0.8;  //1026
-
-    xiuzhengCs=0;
-    //    if (parkMode)
-    //    {
-
-
-    //        handBrakePark(gps_decition,10000,now_gps_ins);
-    //        return gps_decition;
-    //    }
-
-
-
-
-
-    realspeed = now_gps_ins.speed;
-
-    secSpeed = realspeed / 3.6;
-
-
-
-
-    //sidePark
-
-    if(ServiceCarStatus.mnParkType==1){
-
-        if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
-            int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
-            if(bocheEN==1){
-                ServiceCarStatus.bocheEnable=1;
-            }else if(bocheEN==0){
-                ServiceCarStatus.bocheEnable=0;
-            }
-        }else{
-            ServiceCarStatus.bocheEnable=2;
-        }
-
-
-
-        if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&
-                vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4&&  vehState!=reverseArr ){
-            if(abs(realspeed)>0.1){
-                dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-                gps_decition->wheel_angle=0;
-                return gps_decition;
-            }else{
-                if(trumpet()>3000){
-                    trumpetFirstCount=true;
-                    vehState=dRever;
-                }
-
-            }
-            //         ServiceCarStatus.bocheMode=false;
-        }
-
-    }
-
-
-    //chuizhiPark
-
-    if(ServiceCarStatus.mnParkType==0){
-        if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr ){
-            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
-            if(bocheEN==1){
-                ServiceCarStatus.bocheEnable=1;
-            }else if(bocheEN==0){
-                ServiceCarStatus.bocheEnable=0;
-            }
-        }else{
-            ServiceCarStatus.bocheEnable=2;
-        }
-
-
-
-        if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect&&  vehState!=reverseArr ){
-
-            if(abs(realspeed)>0.1){
-                dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-                gps_decition->wheel_angle=0;
-                return gps_decition;
-            }else{
-                if(trumpet()>3000){
-                    trumpetFirstCount=true;
-                    vehState=reverseCar;
-                }
-
-            }
-            //     ServiceCarStatus.bocheMode=false;
-
-        }
-    }
-    // ChuiZhiTingChe
-    if (vehState == reverseCar)
-    {
-        Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
-
-        vehState = reversing;
-        qiedianCount=true;
-
-
-    }
-    if (vehState == reversing)
-    {
-        double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
-        if (dis<2.0)//0.5
-        {
-            vehState = reverseCircle;
-            qiedianCount = true;
-            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
-        }
-        else {
-            controlAng = 0;
-            dSpeed = 2;
-            dSecSpeed = dSpeed / 3.6;
-            gps_decition->wheel_angle = 0;
-            gps_decition->speed = dSpeed;
-            obsDistance=-1;
-            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            //	gps_decition->accelerator = 0;
-            return gps_decition;
-        }
-    }
-
-    if (vehState == reverseCircle)
-    {
-        double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
-        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
-        if((((angdis<5)||(angdis>355)))&&(dis<3.0))
-            //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
-        {
-            vehState = reverseDirect;
-            qiedianCount = true;
-            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
-
-        }
-        else {
-            if (qiedianCount && trumpet()<0)
-                //         if (qiedianCount && trumpet()<1500)
-            {
-
-                //                gps_decition->brake = 10;
-                //                gps_decition->torque = 0;
-                dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            }
-            else
-            {
-                qiedianCount = false;
-                trumpetFirstCount = true;
-
-
-                dSpeed = 2;
-                dSecSpeed = dSpeed / 3.6;
-                gps_decition->speed = dSpeed;
-                obsDistance=-1;
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-
-            }
-
-            controlAng = Compute00().bocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng*1.05;
-
-            cout<<"farTpointDistance================"<<dis<<endl;
-
-            return gps_decition;
-        }
-    }
-
-    if (vehState == reverseDirect)
-    {
-        //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
-        Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
-
-        if ((pt.y)<0.5)
-        {
-
-            qiedianCount=true;
-            vehState=reverseArr;
-            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
-            //            gps_decition->accelerator = -3;
-            //            gps_decition->brake = 10;
-            //            gps_decition->torque = 0;
-            gps_decition->wheel_angle=0;
-            dSpeed=0;
-            minDecelerate=min(minDecelerate,-0.5f);
-            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            return gps_decition;
-
-
-
-
-        }
-        else {
-
-            //if (qiedianCount && trumpet()<0)
-            if (qiedianCount && trumpet()<1000)
-            {
-
-                //                gps_decition->brake = 10;
-                //                gps_decition->torque = 0;
-                dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            }
-            else
-            {
-                qiedianCount = false;
-                trumpetFirstCount = true;
-                dSpeed = 1;
-                dSecSpeed = dSpeed / 3.6;
-                gps_decition->speed = dSpeed;
-                obsDistance=-1;
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            }
-
-
-
-            controlAng = 0;
-
-            gps_decition->wheel_angle = 0;
-
-            return gps_decition;
-        }
-    }
-
-
-
-    if (vehState == reverseArr)
-    {
-        //
-
-        ServiceCarStatus.bocheMode=false;
-        if (qiedianCount && trumpet()<1500)
-        {
-
-            //            gps_decition->brake = 10;
-            //            gps_decition->torque = 0;
-            dSpeed=0;
-            minDecelerate=min(minDecelerate,-0.5f);
-            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            ServiceCarStatus.bocheMode=false;
-        }
-        else
-        {
-            qiedianCount = false;
-            trumpetFirstCount = true;
-            ServiceCarStatus.bocheEnable=0;
-            vehState=normalRun;
-            ServiceCarStatus.mbRunPause=true;
-            ServiceCarStatus.mnBocheComplete=100;
-            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
-        }
-
-
-        gps_decition->wheel_angle = 0 ;
-
-
-
-        return gps_decition;
-
-    }
-
-
-
-    //ceFangWeiBoChe
-
-    if (vehState == dRever)
-    {
-        GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-
-        Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
-
-        vehState = dRever0;
-        qiedianCount=true;
-
-
-        std::cout<<"enter side boche mode"<<std::endl;
-
-
-
-    }
-    if (vehState == dRever0)
-    {
-        double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
-        if (dis<2.0)
-        {
-            vehState = dRever1;
-            qiedianCount = true;
-            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
-        }
-        else {
-            controlAng = 0;
-            dSpeed = 2;
-            dSecSpeed = dSpeed / 3.6;
-            gps_decition->wheel_angle = 0;
-            gps_decition->speed = dSpeed;
-            obsDistance=-1;
-            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            //	gps_decition->accelerator = 0;
-            return gps_decition;
-        }
-    }
-
-
-    if (vehState == dRever1)
-    {
-        double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);
-
-        if(dis<2.0)
-        {
-            vehState = dRever2;
-            qiedianCount = true;
-            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
-
-        }
-        else {
-            if (qiedianCount && trumpet()<1000)
-
-            {
-
-                //                gps_decition->brake = 10;
-                //                gps_decition->torque = 0;
-                dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            }
-            else
-            {
-                qiedianCount = false;
-                trumpetFirstCount = true;
-
-
-                dSpeed = 2;
-                dSecSpeed = dSpeed / 3.6;
-                gps_decition->speed = dSpeed;
-                obsDistance=-1;
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-
-            }
-
-            controlAng = Compute00().dBocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng;
-
-            cout<<"farTpointDistance================"<<dis<<endl;
-
-            return gps_decition;
-        }
-    }
-
-
-    if (vehState == dRever2)
-    {
-        double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
-        Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
-        Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
-        double xx= (pt1.x-pt2.x);
-        // if(xx>0)
-        if(xx>-0.5)
-        {
-            GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-            Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
-            double xxx=ptt.x;
-            double yyy =ptt.y;
-            //            if(xxx<-1.5||xx>1){
-            //                int a=0;
-            //            }
-            vehState = dRever3;
-            qiedianCount = true;
-            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
-            cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
-            cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;
-
-
-        }
-        else {
-            if (qiedianCount && trumpet()<1000)
-
-            {
-
-                /*  gps_decition->brake = 10;
-                gps_decition->torque = 0;  */
-                dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            }
-            else
-            {
-                qiedianCount = false;
-                trumpetFirstCount = true;
-
-
-                dSpeed = 2;
-                dSecSpeed = dSpeed / 3.6;
-                gps_decition->speed = dSpeed;
-                obsDistance=-1;
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-
-            }
-
-
-            gps_decition->wheel_angle = 0 ;
-
-            cout<<"farTpointDistance================"<<dis<<endl;
-
-            return gps_decition;
-        }
-    }
-
-
-    if (vehState == dRever3)
-    {
-        double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
-        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
-        if((((angdis<5)||(angdis>355)))&&(dis<10.0))
-        {
-            vehState = dRever4;
-            qiedianCount = true;
-            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
-
-        }
-        else {
-            if (qiedianCount && trumpet()<1000)
-
-            {
-
-                //                gps_decition->brake = 10;
-                //                gps_decition->torque = 0;
-                dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            }
-            else
-            {
-                qiedianCount = false;
-                trumpetFirstCount = true;
-
-
-                dSpeed = 2;
-                dSecSpeed = dSpeed / 3.6;
-                gps_decition->speed = dSpeed;
-                obsDistance=-1;
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-
-            }
-
-            controlAng = 0-Compute00().dBocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng*0.95;
-
-            cout<<"farTpointDistance================"<<dis<<endl;
-
-            return gps_decition;
-        }
-    }
-
-
-
-    if (vehState == dRever4)
-    {
-        //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
-        Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
-
-        if ((pt.y)<0.5)
-        {
-
-            qiedianCount=true;
-            vehState=reverseArr;
-            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
-            //            gps_decition->accelerator = -3;
-            //            gps_decition->brake =10 ;
-            dSpeed=0;
-            minDecelerate=min(minDecelerate,-0.5f);
-            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-
-            gps_decition->wheel_angle=0;
-            return gps_decition;
-
-
-
-
-        }
-        else {
-
-            //if (qiedianCount && trumpet()<0)
-            if (qiedianCount && trumpet()<1000)
-            {
-
-                //                gps_decition->brake = 10;
-                //                gps_decition->torque = 0;
-                dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            }
-            else
-            {
-                qiedianCount = false;
-                trumpetFirstCount = true;
-                dSpeed = 2;
-                dSecSpeed = dSpeed / 3.6;
-                gps_decition->speed = dSpeed;
-                obsDistance=-1;
-                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-            }
-
-
-
-            controlAng = 0;
-
-            gps_decition->wheel_angle = 0;
-
-            return gps_decition;
-        }
-    }
-
-
-
-    if (vehState == reverseArr)
-    {
-        //
-        ServiceCarStatus.bocheMode=false;
-
-        if (qiedianCount && trumpet()<1500)
-        {
-
-            //            gps_decition->brake = 10;
-            //            gps_decition->torque = 0;
-            dSpeed=0;
-            minDecelerate=min(minDecelerate,-0.5f);
-            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
-        }
-        else
-        {
-            qiedianCount = false;
-            trumpetFirstCount = true;
-            ServiceCarStatus.bocheEnable=0;
-            vehState=normalRun;
-            ServiceCarStatus.mbRunPause=true;
-            ServiceCarStatus.mnBocheComplete=100;
-            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
-        }
-
-
-        gps_decition->wheel_angle = 0 ;
-
-
-
-        return gps_decition;
-
-    }
-
-
-
-
-
-
-
-
-
-
-
-    obsDistance = -1;
-    esrIndex = -1;
-    lidarDistance = -1;
-
-    obsDistanceAvoid = -1;
-    esrIndexAvoid = -1;
-    roadPre = -1;
-
-
-
-
-
-
-
-
-
-    avoidX = (roadOri-roadNow)*roadWidth;
-
-    gpsTraceOri.clear();
-    gpsTraceNow.clear();
-    gpsTraceAim.clear();
-    gpsTraceAvoid.clear();
-    gpsTraceBack.clear();
-
-
-
-
-
-
-
-    if (!isFirstRun)
-    {
-        //   PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
-        //    if(PathPoint<0){
-        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
-        //    }
-
-    }
-
-
-    if (PathPoint<0)
-    {
-
-        minDecelerate=-1.0;
-        phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
-        return gps_decition;
-
-    }
-
-    DecideGps00::lastGpsIndex = PathPoint;
-    //2020-01-03, kkcai
-    //if(!circleMode && PathPoint>gpsMapLine.size()-200){
-    if(!circleMode && PathPoint>gpsMapLine.size()-100){
-        minDecelerate=-1.0;
-        std::cout<<"map finish brake"<<std::endl;
-    }
-
-
-
-
-    roadOri = gpsMapLine[PathPoint]->roadOri;
-
-    roadSum = gpsMapLine[PathPoint]->roadSum;
-
-
-    if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
-                                                ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
-    {
-        vehState = normalRun;
-        roadNow = roadOri;
-    }
-
-
-
-    //    if (PathPoint!=-1&&((now_gps_ins.rtk_status==6)||(now_gps_ins.rtk_status==5))&&GetDistance(now_gps_ins,*gpsMapLine[PathPoint])<30)
-    //    {
-    //        DecideGps00::lastGpsIndex = PathPoint;
-    //        gpsMissCount = 0;
-
-    //    }
-    //    else
-    //    {
-    //        gpsMissCount++;
-    //    }
-
-    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
-    //    gpsTraceOri = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
-    if(gpsTraceOri.empty()){
-        gps_decition->wheel_angle = 0;
-        gps_decition->speed = dSpeed;
-
-        gps_decition->accelerator = -0.5;
-        gps_decition->brake=10;
-        return gps_decition;
-    }
-
-
-
-    traceDevi=gpsTraceOri[0].x;
-
-    //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
-    //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
-    //    if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
-    //        startingFlag = false;
-    //    }
-    startingFlag = false;
-    if(startingFlag){
-        //        gpsTraceAim = gpsTraceOri;
-        if(abs(gpsTraceOri[0].x)>1){
-            cout<<"frenetPlanner->getPath:pre"<<endl;
-            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
-            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
-            if(gpsTraceNow.size()==0){
-                gps_decition->accelerator = 0;
-                gps_decition->brake=10;
-                gps_decition->wheel_angle = lastAngle;
-                gps_decition->speed = 0;
-                return gps_decition;
-            }
-        }else{
-            startingFlag = false;
-        }
-    }
-
-
-    if(useFrenet){
-        //如果车辆在变道中，启用frenet规划局部路径
-        if(vehState == avoiding || vehState == backOri){
-            avoidX = (roadOri - roadNow)*roadWidth;
-            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
-            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
-            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
-            if(gpsTraceNow.size()==0){
-                gps_decition->accelerator = 0;
-                gps_decition->brake=10;
-                gps_decition->wheel_angle = lastAngle;
-                gps_decition->speed = 0;
-                return gps_decition;
-            }
-        }
-    }
-
-    if(gpsTraceNow.size()==0){
-        if (roadNow==roadOri)
-        {
-            gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
-            //gpsTraceNow = getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex);
-        }else
-        {
-            //	gpsTraceNow = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-roadNow))], lastGpsIndex);
-            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //gpsTraceNow = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
-        }
-    }
-
-
-    //  dSpeed = getSpeed(gpsTraceNow);
-    dSpeed = 80;
-
-    planTrace = gpsTraceNow; //Add By YuChuli, 2020.11.26
-
-    dSpeed = limitSpeed(controlAng, dSpeed);
-
-
-
-    controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
-    if(!circleMode && PathPoint>(gpsMapLine.size()-100)){
-        controlAng=0;
-    }
-
-
-
-    //1220
-    if(ServiceCarStatus.daocheMode){
-        controlAng=0-controlAng;
-    }
-
-    //2020-0106
-    if(vehState==avoiding){
-        controlAng=max(-150.0,controlAng);
-        controlAng=min(150.0,controlAng);
-    }
-    if(vehState==backOri){
-        controlAng=max(-120.0,controlAng);
-        controlAng=min(120.0,controlAng);
-    }
-
-    //准备驻车
-
-    if (readyZhucheMode)
-    {
-
-        cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
-        cout<<"zhuche point : "<<zhuchePointNum<<endl;
-
-        double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
-
-        if (dis<35)
-        {
-            Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
-            double zhucheDistance = pt.y;
-#if 0
-
-            if (dSpeed > 15)
-            {
-                dSpeed = 15;
-            }
-
-
-
-            if (zhucheDistance < 20 && dis < 25)
-            {
-                dSpeed = min(dSpeed, 5.0);
-            }
-#else
-            if (dSpeed > 12)
-            {
-                dSpeed = 12;
-            }
-
-
-
-            if (zhucheDistance < 9 && dis < 9)
-            {
-                dSpeed = min(dSpeed, 5.0);
-            }
-#endif
-
-
-            if (zhucheDistance < 3.0 && dis < 3.5)
-            {
-                dSpeed = min(dSpeed, 5.0);
-                zhucheMode = true;
-                readyZhucheMode = false;
-                cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
-
-                //1125
-                //                 gps_decition->brake=20;
-                //                 gps_decition->accelerator = -3;
-                //                 gps_decition->wheel_angle = 0-controlAng;
-                //                 gps_decition->speed = 0;
-                //                 gps_decition->torque=0;
-                //                 return gps_decition;
-
-
-
-
-
-            }
-
-        }
-
-    }
-
-
-
-
-
-    if (readyParkMode)
-    {
-        double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
-        hasZhuched = true;
-
-        if (parkDis < 25)
-        {
-            Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
-            double parkDistance = pt.y;
-
-
-            if (dSpeed > 15)
-            {
-                dSpeed = 15;
-            }
-
-            //dSpeed = 15;//////////////////////////////
-
-            if (parkDistance < 15 && parkDistance >= 4.5 && parkDis<20)
-            {
-                dSpeed = min(dSpeed, 8.0);
-            }else if (parkDistance < 4.5 && parkDistance >= 3.5 && parkDis<5.0){
-                dSpeed = min(dSpeed, 5.0);
-            }else if(parkDistance < 3.5 && parkDis<4.0){
-                dSpeed = min(dSpeed, 3.0);
-            }
-
-
-            //            float stopDis=2;
-            //            if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
-            //                stopDis=1.6;
-            //            } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
-            //                stopDis=1.8;
-            //            }
-
-            if (parkDistance < 1.6  && parkDis<2.0)
-            {
-                // dSpeed = 0;
-                parkMode = true;
-                readyParkMode = false;
-            }
-
-
-        }
-    }
-
-
-
-    if (gpsMapLine[PathPoint]->roadMode ==0)
-    {
-        //dSpeed = min(10.0,dSpeed);
-
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = false;
-
-    }else  if (gpsMapLine[PathPoint]->roadMode == 5)
-    {
-        //dSpeed = min(8.0,dSpeed);
-
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = false;
-
-    }else if (gpsMapLine[PathPoint]->roadMode == 11)
-    {
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
-
-    }else if (gpsMapLine[PathPoint]->roadMode == 14)
-    {
-        //dSpeed = min(8.0,dSpeed);
-
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
-        gps_decition->leftlamp = true;
-        gps_decition->rightlamp = false;
-    }else if (gpsMapLine[PathPoint]->roadMode == 15)
-    {
-        //dSpeed = min(8.0,dSpeed);
-
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = true;
-    }else if (gpsMapLine[PathPoint]->roadMode == 16)
-    {
-        // dSpeed = min(10.0,dSpeed);
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
-        gps_decition->leftlamp = true;
-        gps_decition->rightlamp = false;
-    }else if (gpsMapLine[PathPoint]->roadMode == 17)
-    {
-        //  dSpeed = min(10.0,dSpeed);
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = true;
-    }else if (gpsMapLine[PathPoint]->roadMode == 18)
-    {
-        // dSpeed = min(10.0,dSpeed);
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
-        /*if(gpsMapLine[PathPoint]->light_left_or_right == 1)
-        {
-            gps_decition->leftlamp = true;
-            gps_decition->rightlamp = false;
-        }
-        else if(gpsMapLine[PathPoint]->light_left_or_right == 2)
-        {
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = true;
-        }*/
-    }else if (gpsMapLine[PathPoint]->roadMode == 1)
-    {
-        dSpeed = min(10.0,dSpeed);
-    }else if (gpsMapLine[PathPoint]->roadMode == 2)
-    {
-        dSpeed = min(15.0,dSpeed);
-    }
-    else if (gpsMapLine[PathPoint]->roadMode == 7)
-    {
-        dSpeed = min(15.0,dSpeed);
-        xiuzhengCs=1.5;
-    }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
-    {
-        dSpeed = min(4.0,dSpeed);
-
-    }
-
-
-
-
-
-    if (gpsMapLine[PathPoint]->speed_mode == 2)
-    {
-        dSpeed = min(25.0,dSpeed);
-
-    }
-    dSecSpeed = dSpeed / 3.6;
-
-
-
-
-
-
-    std::cout<<"juecesudu2="<<dSpeed<<std::endl;
-
-
-
-
-
-
-
-    if(vehState==changingRoad){
-        if(gpsTraceNow[0].x>1.0){
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = true;
-        }else if(gpsTraceNow[0].x<-1.0){
-            gps_decition->leftlamp = true;
-            gps_decition->rightlamp = false;
-        }else{
-            vehState==normalRun;
-        }
-    }
-
-    //    qDebug("decide0 time is %d",xTime.elapsed());
-
-    //1220
-    if(!ServiceCarStatus.daocheMode){
-        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-    }else{
-        gpsTraceRear.clear();
-        for(int i=0;i<gpsTraceNow.size();i++){
-            Point2D pt;
-            pt.x=0-gpsTraceNow[i].x;
-            pt.y=0-gpsTraceNow[i].y;
-            gpsTraceRear.push_back(pt);
-        }
-        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        //  obsDistance=-1; //1227
-    }
-
-
-
-
-    //old_bz--------------------------------------------------------------------------------------------------
-
-    if (vehState == avoiding)
-    {
-        cout<<"\n==================avoiding=================\n"<<endl;
-        //  vehState=normalRun; //1025
-        if (dSpeed > 10) {
-            dSpeed = 10;
-        }
-        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
-        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
-            vehState = normalRun;
-            //            useFrenet = false;
-            //            useOldAvoid = true;
-        }
-        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
-            // vehState = avoidObs; 0929
-            vehState = normalRun;
-        }
-        else if (gpsTraceNow[0].x>0)
-        {
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = true;
-        }
-        else if(gpsTraceNow[0].x<0)
-        {
-            gps_decition->leftlamp = true;
-            gps_decition->rightlamp = false;
-        }
-    }
-
-
-    if (vehState==preBack)
-    {
-        vehState = backOri;
-    }
-
-
-    if (vehState==backOri)
-    {
-        cout<<"\n================backOri===========\n"<<endl;
-        //  vehState=normalRun; //1025
-        if (dSpeed > 10) {
-            dSpeed = 10;
-        }
-
-        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
-        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
-            vehState = normalRun;
-            //            useFrenet = false;
-            //            useOldAvoid = true;
-        }
-        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
-            // vehState = avoidObs; 0929
-            vehState = normalRun;
-        }
-        else if (gpsTraceNow[0].x>0)
-        {
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = true;
-        }
-        else if (gpsTraceNow[0].x<0)
-        {
-            gps_decition->leftlamp = true;
-            gps_decition->rightlamp = false;
-        }
-    }
-
-    std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
-
-    cout<<"\n当前RoadNum:%d\n"<<roadNow<<endl;
-
-
-
-
-
-
-
-
-
-    //   计算回归原始路线
-
-    if (roadNow!=roadOri)
-    {
-        //        useFrenet = true;
-        //        useOldAvoid = false;
-
-        if(useFrenet){
-            if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
-            {
-                double offsetL = -(roadSum - 1)*roadWidth;
-                double offsetR = (roadOri - 0)*roadWidth;
-                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
-            }
-        }
-        else if(useOldAvoid){
-            roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
-            avoidX = (roadOri - roadNow)*roadWidth; //1012
-        }
-    }
-    if (roadNow != roadOri && roadPre!=-1)
-    {
-
-        roadNow = roadPre;
-        avoidX = (roadOri - roadNow)*roadWidth;
-        gpsTraceNow.clear();
-        if(useOldAvoid){
-            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-        }
-        else if(useFrenet){
-            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
-        }
-
-        vehState = backOri;
-        hasCheckedBackLidar = false;
-        //  checkBackObsTimes = 0;
-
-        cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
-
-    }
-
-    //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.socfDis+20.0) &&(abs(realspeed)<1.0) &&
-            (vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
-            && (gpsMapLine[PathPoint]->runMode==1)){
-        ObsTimeStart=GetTickCount();
-        cout<<"\n====================preAvoid time count start==================\n"<<endl;
-    }
-    if(ObsTimeStart!=-1){
-        ObsTimeEnd=GetTickCount();
-    }
-    if(ObsTimeEnd!=-1){
-        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
-    }
-    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
-        vehState=avoidObs;
-        ObsTimeStart=-1;
-        ObsTimeEnd=-1;
-        ObsTimeSpan=-1;
-        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
-    }
-
-    if(obsDistance<0 ||obsDistance>ServiceCarStatus.socfDis){
-        ObsTimeStart=-1;
-        ObsTimeEnd=-1;
-        ObsTimeSpan=-1;
-
-    }
-
-
-    //避障模式
-
-
-    if (vehState == avoidObs   || vehState==waitAvoid ) {
-
-        //      if (obsDistance <20 && obsDistance >0)
-        if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
-        {
-            dSpeed = min(7.0,dSpeed);
-            avoidTimes++;
-            //          if (avoidTimes>=6)
-            if (avoidTimes>=ServiceCarStatus.aocfTimes)
-            {
-                vehState = preAvoid;
-                avoidTimes = 0;
-            }
-
-        }
-        //        else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
-        //        {
-        //            dSpeed = 10;
-        //            avoidTimes = 0;
-        //        }
-        else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
-        {
-            dSpeed =  min(15.0,dSpeed);
-            avoidTimes = 0;
-        }
-        else
-        {
-            avoidTimes = 0;
-        }
-
-    }
-
-
-    if (vehState == preAvoid)
-    {
-        cout<<"\n====================preAvoid==================\n"<<endl;
-        /* if (obsDisVector[roadNow]>30)*/  //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
-        if (obsDisVector[roadNow]>ServiceCarStatus.aocfDis)
-        {
-            // vehState = avoidObs; 0929
-            vehState=normalRun;
-        }
-        else
-        {
-            //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
-            if(useOldAvoid){
-                roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
-                avoidX = (roadOri - roadNow)*roadWidth;  //1012
-            }
-            else if(useFrenet){
-                double offsetL = -(roadSum - 1)*roadWidth;
-                double offsetR = (roadOri - 0)*roadWidth;
-                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
-            }
-
-            if (roadPre == -1 && obsDisVector[roadNow]<1.5 && obsDisVector[roadNow]>0)
-            {
-                //  vehState = waitAvoid; 0929
-                vehState = avoidObs;
-            }
-            else if (roadPre != -1)
-            {
-                if(useOldAvoid){
-                    roadNow = roadPre;
-                    avoidX = (roadOri - roadNow)*roadWidth;
-                    gpsTraceNow.clear();
-                    gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-                }
-                else if(useFrenet){
-                    if(roadPre != roadNow){
-                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + Veh_Lenth;
-                        startAvoidGpsInsVector[roadNow] = now_gps_ins;
-                    }
-                    roadNow = roadPre;
-                    avoidX = (roadOri - roadNow)*roadWidth;
-                    gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
-                }
-
-
-                vehState = avoiding;
-
-                hasCheckedAvoidLidar = false;
-
-                cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;
-
-            }
-        }
-    }
-
-    //--------------------------------------------------------------------------------old_bz_end
-
-
-
-
-
-    //TOUCHEPINGBI
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-    if (parkMode)
-    {
-        minDecelerate=-1.0;
-
-        if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
-            parkMode=false;
-        }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>20){
-            parkMode=false;
-        }
-
-    }
-
-
-
-    //驻车
-
-    if (zhucheMode)
-    {
-        int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;
-
-        //        if(trumpet()<16000)
-        if (trumpet()<mzcTime)
-        {
-            //            if (trumpet()<12000){
-            cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
-            minDecelerate=-1.0;
-            if(abs(realspeed)<0.2){
-                controlAng=0;  //tju
-            }
-        }
-        else
-        {
-            hasZhuched = true; //1125
-            zhucheMode = false;
-            trumpetFirstCount = true;
-            cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
-        }
-
-    }
-
-
-    //判断驻车标志位
-    if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
-    {
-        hasZhuched = false;
-    }
-
-
-
-
-
-
-
-
-
-
-
-    if ( vehState==changingRoad || vehState==chaocheBack)
-    {
-        double lastAng = 0.0 - lastAngle;
-        if (controlAng>40)
-        {
-            controlAng =40;
-        }
-        else if (controlAng<-40)
-        {
-            controlAng = - 40;
-        }
-
-
-    }
-
-
-    //速度结合角度的限制
-    controlAng = limitAngle(realspeed, controlAng);
-
-
-    std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;
-
-
-
-    //1220
-    if(PathPoint+80<gpsMapLine.size()-1){
-        if(gpsMapLine[PathPoint+80]->roadMode==4  && !ServiceCarStatus.daocheMode){
-            changingDangWei=true;
-        }
-    }
-
-    if(changingDangWei){
-        if(abs(realspeed)>0.1){
-            dSpeed=0;
-        }else{
-            dSpeed=0;
-            gps_decition->dangWei=2;
-            ServiceCarStatus.daocheMode=true;
-        }
-    }
-    //1220 end
-
-
-
-
-    for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
-    {
-        GPS_INS gpsIns;
-        GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude,  &gpsIns.gps_x, &gpsIns.gps_y);
-
-        double dis = GetDistance(gpsIns, now_gps_ins);
-        if(dis<20)
-            ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
-    }
-
-
-
-
-    if (ServiceCarStatus.carState == 0 && busMode)
-    {
-
-        minDecelerate=-1.0;
-    }
-
-
-
-    if (ServiceCarStatus.carState == 3 && busMode)
-    {
-
-        if(realspeed<0.1){
-            ServiceCarStatus.carState=0;
-            minDecelerate=-1.0;
-        }else{
-            nearStation=true;
-            dSpeed=0;
-        }
-
-    }
-
-
-
-
-
-
-
-    ///kkcai, 2020-01-03
-    //if (ServiceCarStatus.carState == 2 && busMode)
-    if (ServiceCarStatus.carState == 2)
-    {
-
-        aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
-        aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
-        //   gps2xy(ServiceCarStatus.amilat, ServiceCarStatus.amilng, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-        std::cout<<"lng"<<ServiceCarStatus.amilng<<std::endl;
-        qDebug("lat:%f", aim_gps_ins.gps_lat);
-        qDebug("lon:%f", aim_gps_ins.gps_lng);
-
-        std::cout<<"lat"<<ServiceCarStatus.amilat<<std::endl;
-        double dis = GetDistance(aim_gps_ins, now_gps_ins);
-        Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
-
-        std::cout << "\n呼车dis距离:%f\n" << dis << std::endl;
-        std::cout << "\n呼车Y距离:%f\n" << pt.y << std::endl;
-
-        //         if (dis<20 && pt.y<8&& realspeed<0.1)
-        if (dis<20 && pt.y<5 && abs(pt.x)<3)
-        {
-            dSpeed = 0;
-            nearStation=true;
-            //is_arrivaled = true;
-            //ServiceCarStatus.status[0] = true;
-            ServiceCarStatus.istostation=1;
-            minDecelerate=-1.0;
-        }
-
-        else if (dis<20 && pt.y<15 && abs(pt.x)<3)
-        {
-            nearStation=true;
-            dSpeed = min(8.0, dSpeed);
-        }
-        else if (dis<30 && pt.y<20 && abs(pt.x)<3)
-        {
-            dSpeed = min(15.0, dSpeed);
-        }
-
-        else if (dis<50 && abs(pt.x)<3)
-        {
-            dSpeed = min(20.0, dSpeed);
-        }
-    }
-
-
-    dSecSpeed = dSpeed / 3.6;
-    gps_decition->speed = dSpeed;
-
-
-
-    if (gpsMapLine[PathPoint]->runMode == 2)
-    {
-        obsDistance = -1;
-
-    }
-
-    std::cout<<"juecesudu0="<<dSpeed<<std::endl;
-
-    //-------------------------------traffic light----------------------------------------------------------------------------------------
-
-    if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
-        traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
-        //    traffic_light_pathpoint=130;
-        float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
-                                                     traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
-        dSpeed = min((double)traffic_speed,dSpeed);
-        if(traffic_speed==0){
-            minDecelerate=-2.0;
-        }
-    }
-    //-------------------------------traffic light  end-----------------------------------------------------------------------------------------------
-
-
-    //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------
-
-
-    double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight,  gpsMapLine, v2xTrafficVector,
-                                                         PathPoint, secSpeed, dSpeed,  circleMode);
-
-
-    dSpeed = min(v2xTrafficSpeed,dSpeed);
-
-    //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
-
-
-    if(obsDistance>0 && obsDistance<10){
-        dSpeed=0;
-    }
-    //  obsDistance=-1; //1227
-
-    if(ServiceCarStatus.group_control){
-        dSpeed = ServiceCarStatus.group_comm_speed;
-    }
-    if(dSpeed==0){
-        minDecelerate=min(-1.0f,minDecelerate);
-    }
-
-    std::cout<<"dSpeed= "<<dSpeed<<std::endl;
-
-    // givlog->debug("SPEED","dSpeed is %f",dSpeed);
-    phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
-
-
-
-
-
-    gps_decition->wheel_angle = 0.0 - controlAng;
-
-
-
-    lastAngle=gps_decition->wheel_angle;
-
-    //   qDebug("decide1 time is %d",xTime.elapsed());
-
-    return gps_decition;
-}
-
-
-
-void iv::decition::DecideGps00::initMethods(){
-
-    pid_Controller= new PIDController();
-    ge3_Adapter = new Ge3Adapter();
-    qingyuan_Adapter = new QingYuanAdapter();
-    vv7_Adapter = new VV7Adapter();
-    zhongche_Adapter = new ZhongcheAdapter();
-    bus_Adapter = new BusAdapter();
-    hapo_Adapter = new HapoAdapter();
-
-
-    mpid_Controller.reset(pid_Controller);
-    mge3_Adapter.reset(ge3_Adapter);
-    mqingyuan_Adapter.reset(qingyuan_Adapter);
-    mvv7_Adapter.reset(vv7_Adapter);
-    mzhongche_Adapter.reset(zhongche_Adapter);
-    mbus_Adapter.reset(bus_Adapter);
-    mhapo_Adapter.reset(hapo_Adapter);
-
-    frenetPlanner = new FrenetPlanner();
-    //    laneChangePlanner = new LaneChangePlanner();
-
-    mfrenetPlanner.reset(frenetPlanner);
-    //    mlaneChangePlanner.reset(laneChangePlanner);
-
-}
-
-
-void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {
-
-    pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);
-
-    if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
-        ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
-    }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
-        qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
-    }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
-        vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
-    }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
-        zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
-    }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
-        bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
-    }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
-        hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
-    }
-}
-
-
-
-std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
-    vector<iv::Point2D> trace;
-    //	int  PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
-    /*if (PathPoint != -1)
-        lastGpsIndex = PathPoint;*/
-
-    if (gpsMapLine.size() > 600 && PathPoint >= 0) {
-        int aimIndex;
-        if(circleMode){
-            aimIndex=PathPoint+600;
-        }else{
-            aimIndex=min((PathPoint+600),(int)gpsMapLine.size());
-        }
-
-        for (int i = PathPoint; i < aimIndex; i++)
-        {
-            int index = i % gpsMapLine.size();
-            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
-            pt.x += offset_real * 0.032;
-            pt.v1 = (*gpsMapLine[index]).speed_mode;
-            pt.v2 = (*gpsMapLine[index]).mode2;
-            pt.roadMode=(*gpsMapLine[index]).roadMode;
-            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
-            {
-                readyZhucheMode = true;
-                DecideGps00::zhuchePointNum = index;
-            }
-
-
-
-            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
-            {
-                readyZhucheMode = true;
-                DecideGps00::zhuchePointNum = index;
-            }
-
-            //csvv7
-            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
-            {
-                readyParkMode = true;
-                DecideGps00::finishPointNum = index;
-            }
-
-            //			if (pt.v1 == 22 || pt.v1 == 23)
-            //			{
-            //				readyParkMode = true;
-            //				DecideGps00::finishPointNum = i;
-            //			}
-
-
-            switch (pt.v1)
-            {
-            case 0:
-                pt.speed = 80;
-                break;
-            case 1:
-                pt.speed = 25;
-                break;
-            case 2:
-                pt.speed =25;
-                break;
-            case 3:
-                pt.speed = 20;
-                break;
-            case 4:
-                pt.speed =18;
-                break;
-            case 5:
-                pt.speed = 18;
-                break;
-            case 7:
-                pt.speed = 10;
-                break;
-            case 22:
-                pt.speed = 5;
-                break;
-            case 23:
-                pt.speed = 5;
-                break;
-            default:
-                break;
-            }
-            trace.push_back(pt);
-        }
-    }
-    return trace;
-}
-
-std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
-
-    if (offset==0)
-    {
-        return gpsTrace;
-    }
-
-    std::vector<iv::Point2D> trace;
-    for (int j = 0; j < gpsTrace.size(); j++)
-    {
-        double sumx1 = 0, sumy1 = 0, count1 = 0;
-        double sumx2 = 0, sumy2 = 0, count2 = 0;
-        for (int k = max(0, j - 4); k <= j; k++)
-        {
-            count1 = count1 + 1;
-            sumx1 += gpsTrace[k].x;
-            sumy1 += gpsTrace[k].y;
-        }
-        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
-        {
-            count2 = count2 + 1;
-            sumx2 += gpsTrace[k].x;
-            sumy2 += gpsTrace[k].y;
-        }
-        sumx1 /= count1; sumy1 /= count1;
-        sumx2 /= count2; sumy2 /= count2;
-
-        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
-
-        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
-        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
-
-        double avoidLenth = abs(offset);
-        if (offset<0)
-        {
-            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
-                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
-            ptLeft.speed = gpsTrace[j].speed;
-            ptLeft.v1 = gpsTrace[j].v1;
-            ptLeft.v2 = gpsTrace[j].v2;
-            trace.push_back(ptLeft);
-        }
-        else
-        {
-            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
-                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
-            ptRight.speed = gpsTrace[j].speed;
-            ptRight.v1 = gpsTrace[j].v1;
-            ptRight.v2 = gpsTrace[j].v2;
-
-
-            trace.push_back(ptRight);
-        }
-    }
-    return trace;
-}
-
-
-double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
-    double angle=0;
-    if ( abs(iv::decition::DecideGps00().minDis) < 20 && abs(iv::decition::DecideGps00().maxAngle) < 100)
-    {
-        //   angle = iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
-        pid_Controller->getControlDecition(gpsIns, gpsTrace,  -1,  -1,  -1, true, false, &decition);
-        angle= decition->wheel_angle;
-    }
-    return angle;
-}
-
-double iv::decition::DecideGps00::getSpeed(std::vector<Point2D> gpsTrace) {
-    double speed=0;
-    int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
-    speed = gpsTrace[speedPoint].speed;
-    for (int i = 0; i < speedPoint; i++) {
-        speed = min(speed, gpsTrace[i].speed);
-    }
-    return speed;
-}
-
-
-
-
-//void iv::decition::DecideGps00::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
-//
-//	if (!obsRadars.empty())
-//	{
-//		esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
-//
-//		if (esrIndex != -1)
-//		{
-//			 esrDistance = obsRadars[esrIndex].nomal_y;
-//
-//
-//
-//			obsSpeed = obsRadars[esrIndex].speed_y;
-//
-//		}
-//		else {
-//			esrDistance = -1;
-//		}
-//
-//	}
-//	else
-//	{
-//		esrIndex = -1;
-//		esrDistance = -1;
-//	}
-//	if (esrDistance < 0) {
-//		ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
-//	}
-//	else {
-//		ODS("\n------------------>ESR障碍物距离:%f   ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
-//	}
-//
-//	ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
-//}
-
-
-
-void iv::decition::DecideGps00::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
-
-
-    int esrPathpoint;
-
-    esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint);
-
-    if (esrIndex != -1)
-    {
-        //优化
-        //        double distance = 0.0;
-        //        for(int i=0; i<esrPathpoint; ++i){
-        //            distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
-        //        }
-        //        esrDistance = distance - Esr_Y_Offset;
-        //        if(esrDistance<=0){
-        //            esrDistance=1;
-        //        }
-
-
-        esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
-        obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
-
-    }
-    else {
-        esrDistance = -1;
-    }
-}
-
-
-
-
-void iv::decition::DecideGps00::getEsrObsDistanceAvoid() {
-
-    esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);
-
-    if (esrIndexAvoid != -1)
-    {
-        esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
-        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
-
-    }
-    else {
-        esrDistanceAvoid = -1;
-    }
-
-    if (esrDistanceAvoid < 0) {
-        std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
-    }
-    else {
-        std::cout << "\nESR障碍物距离Avoid:%f   %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
-    }
-
-    std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
-}
-
-
-
-
-double iv::decition::DecideGps00::limitAngle(double speed, double angle) {
-    double preAngle = angle;
-
-    if (speed > 15)
-    {
-        if (preAngle > 350)
-        {
-            preAngle = 350;
-        }
-        if (preAngle < -350)
-        {
-            preAngle = -350;
-        }
-    }
-    if (speed > 22)
-    {
-        if (preAngle > 200)
-        {
-            preAngle = 200;
-        }
-        if (preAngle < -200)
-        {
-            preAngle = -200;
-        }
-    }
-    if (speed > 25)
-    {
-        if (preAngle > 150)
-        {
-            preAngle = 150;
-        }
-        if (preAngle < -150)
-        {
-            preAngle = -150;
-        }
-    }
-    if (speed > 30)
-    {
-        if (preAngle > 70)
-        {
-            preAngle = 70;
-        }
-        if (preAngle < -70)
-        {
-            preAngle = -70;
-        }
-    }
-    if (speed > 45)                     //20
-    {
-        if (preAngle > 15)
-        {
-            preAngle = 15;
-        }
-        if (preAngle < -15)
-        {
-            preAngle = -15;
-        }
-    }
-    return preAngle;
-}
-
-
-
-double iv::decition::DecideGps00::limitSpeed(double angle, double speed) {
-
-    if (abs(angle) > 500 && speed > 8) speed = 8;
-    else if (abs(angle) > 350 && speed > 14) speed = 14;
-    else if (abs(angle) > 200 && speed > 21) speed = 21;
-    else if (abs(angle) > 150 && speed > 24) speed = 24;
-    else if (abs(angle) > 60 && speed > 29) speed = 29;
-    else if (abs(angle) > 20 && speed > 34) speed = 34;
-    return max(0.0, speed);
-}
-
-
-bool iv::decition::DecideGps00::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {
-
-    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+Veh_Lenth)||
-            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
-    {
-        return false;
-    }
-
-    if (roadNum-roadNow>1)
-    {
-        for (int i = roadNow+1; i < roadNum; i++)
-        {
-            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
-                return false;
-            }
-        }
-
-    }
-
-    else if (roadNow-roadNum>1)
-    {
-        for (int i = roadNow-1; i >roadNum; i--)
-        {
-            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
-                return false;
-            }
-        }
-    }
-    return true;
-}
-
-
-
-bool iv::decition::DecideGps00::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
-    //lsn
-    if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
-    {
-        return false;
-    }
-    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
-            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
-    {
-        return false;
-    }
-
-
-    if (roadNum - roadNow>1)
-    {
-        for (int i = roadNow + 1; i < roadNum; i++)
-        {
-            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
-                return false;
-            }
-        }
-
-    }
-
-    else if (roadNow - roadNum>1)
-    {
-        for (int i = roadNow - 1; i >roadNum; i--)
-        {
-            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
-                return false;
-            }
-        }
-    }
-    return true;
-}
-
-
-void iv::decition::DecideGps00::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
-
-
-
-    if (lidarGridPtr == NULL)
-    {
-        iv::decition::DecideGps00::lidarDistanceAvoid = iv::decition::DecideGps00::lastLidarDisAvoid;
-    }
-    else {
-        obsPointAvoid = Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr);
-        iv::decition::DecideGps00::lastLidarDisAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
-    }
-    std::cout << "\nLidarAvoid距离:%f\n" << iv::decition::DecideGps00::lidarDistanceAvoid << std::endl;
-    getEsrObsDistanceAvoid();
-
-    //lidarDistanceAvoid = -1;   //20200103 apollo_fu
-
-    if (esrDistanceAvoid>0 && iv::decition::DecideGps00::lidarDistanceAvoid > 0)
-    {
-        if (iv::decition::DecideGps00::lidarDistanceAvoid >= esrDistanceAvoid)
-        {
-            obsDistanceAvoid = esrDistanceAvoid;
-            obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
-        }
-        else if (!ServiceCarStatus.obs_radar.empty())
-        {
-            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
-            obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
-            std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
-        }
-        else
-        {
-            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
-            obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
-            std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
-        }
-    }
-    else if (esrDistanceAvoid>0)
-    {
-        obsDistanceAvoid = esrDistanceAvoid;
-        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
-    }
-    else if (iv::decition::DecideGps00::lidarDistanceAvoid > 0)
-    {
-        obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
-        obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
-        std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
-    }
-    else {
-        obsDistanceAvoid = esrDistanceAvoid;
-        obsSpeedAvoid = 0 - secSpeed;
-    }
-
-    std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;
-
-
-
-
-
-    if (iv::decition::DecideGps00::obsDistanceAvoid <0 && obsLostTimeAvoid<4)
-    {
-        iv::decition::DecideGps00::obsDistanceAvoid = iv::decition::DecideGps00::lastDistanceAvoid;
-        obsLostTimeAvoid++;
-    }
-    else
-    {
-        obsLostTimeAvoid = 0;
-        iv::decition::DecideGps00::lastDistanceAvoid = -1;
-    }
-
-
-
-
-    if (obsDistanceAvoid>0)
-    {
-        iv::decition::DecideGps00::lastDistanceAvoid = obsDistanceAvoid;
-    }
-
-    std::cout << "\nODSAvoid距离:%f\n" << iv::decition::DecideGps00::obsDistanceAvoid << std::endl;
-}
-
-void iv::decition::DecideGps00::init() {
-    for (int i = 0; i < roadSum; i++)
-    {
-        lastEsrIdVector.push_back(-1);
-        lastEsrCountVector.push_back(0);
-        GPS_INS gps_ins;
-        gps_ins.gps_x = 0;
-        gps_ins.gps_y = 0;
-        startAvoidGpsInsVector.push_back(gps_ins);
-        avoidMinDistanceVector.push_back(0);
-    }
-}
-
-
-
-#include <QTime>
-
-void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
-                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
-
-    //    QTime xTime;
-    //   xTime.start();
-    //    qDebug("time 0 is %d ",xTime.elapsed());
-    double obs,obsSd;
-    if (lidarGridPtr == NULL)
-    {
-        lidarDistance = lastLidarDis;
-        //	lidarDistance = lastlidarDistance;
-
-    }
-    else {
-        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
-        float lidarXiuZheng=0;
-        if(!ServiceCarStatus.useMobileEye){
-            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
-        }
-        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
-        //    lidarDistance=-1;
-        if (lidarDistance<0)
-        {
-            lidarDistance = -1;
-        }
-        lastLidarDis = lidarDistance;
-    }
-
-
-
-
-
-
-    if(lidarDistance<0){
-        lidarDistance=500;
-    }
-
-
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
-
-    ServiceCarStatus.mLidarObs = lidarDistance;
-    obs = lidarDistance;
-
-    obsSd= obsPoint.obs_speed_y;
-
-
-    if(roadNum==roadNow){
-        obsDistance=obs;
-        obsSpeed=obsSd;
-    }
-
-
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
-
-    ServiceCarStatus.mObs = obsDistance;
-    if(ServiceCarStatus.mObs>100){
-        ServiceCarStatus.mObs =-1;
-    }
-
-    if (obsDistance>0)
-    {
-        lastDistance = obsDistance;
-    }
-
-    //lsn
-    if(obs<0){
-        obsDisVector[roadNum]=500;
-    }else{
-        obsDisVector[roadNum]=obs;
-    }
-
-    //   qDebug("time 3 is %d ",xTime.elapsed());
-
-}
-
-
-
-
-//1220
-void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
-                                                     const std::vector<GPSData> gpsMapLine) {
-
-    double obs,obsSd;
-
-
-    if (lidarGridPtr == NULL)
-    {
-        lidarDistance = lastLidarDis;
-        //	lidarDistance = lastlidarDistance;
-
-    }
-    else {
-        obsPoint = Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
-        float lidarXiuZheng=0;
-        if(!ServiceCarStatus.useMobileEye){
-            lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
-        }
-        if(abs(obsPoint.y)>lidarXiuZheng)
-            lidarDistance = abs(obsPoint.y)-lidarXiuZheng;   //激光距离推到车头 1220
-
-        //    lidarDistance=-1;
-        if (lidarDistance<0)
-        {
-            lidarDistance = -1;
-        }
-        lastLidarDis = lidarDistance;
-    }
-
-
-
-    if(lidarDistance<0){
-        lidarDistance=500;
-    }
-
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
-
-    if(lidarDistance==500){
-        lidarDistance=-1;
-    }
-
-
-
-
-    ServiceCarStatus.mLidarObs = lidarDistance;
-
-    obs=lidarDistance;
-    //	obsSpeed = 0 - secSpeed;
-    obsSd = 0 -secSpeed;
-
-
-
-
-    if(roadNum==roadNow){
-        obsDistance=obs;
-        obsSpeed=obsSd;
-    }
-
-
-    if (obsDistance <0 && obsLostTime<4)
-    {
-        obsDistance = lastDistance;
-        obsLostTime++;
-    }
-    else
-    {
-        obsLostTime = 0;
-        lastDistance = -1;
-    }
-
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
-
-    ServiceCarStatus.mObs = obsDistance;
-    if(ServiceCarStatus.mObs>100){
-        ServiceCarStatus.mObs =-1;
-    }
-
-    if (obsDistance>0)
-    {
-        lastDistance = obsDistance;
-    }
-
-    //lsn
-    if(obs<0){
-        obsDisVector[roadNum]=500;
-    }else{
-        obsDisVector[roadNum]=obs;
-    }
-
-}
-
-
-
-void iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
-    double preObsDis;
-
-    if(!lidar_per.empty()){
-        preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
-        lastPreObsDistance=preObsDis;
-
-    }else{
-        preObsDis=lastPreObsDistance;
-    }
-    if(preObsDis<obsDistance){
-        obsDistance=preObsDis;
-        lastDistance=obsDistance;
-    }
-}
-
-int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
-    roadPre = -1;
-
-    if (roadNow<roadOri)
-    {
-        for (int i = 0; i < roadNow; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
-
-        for (int i = roadOri+1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-        }
-    }
-    else if (roadNow>roadOri)
-    {
-        for (int i = 0; i < roadOri; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-        for (int i = roadNow + 1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
-    }
-
-    else
-    {
-        for (int i = 0; i < roadOri; i++)
-        {
-            gpsTraceAvoid.clear();
-            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-        for (int i = roadOri + 1; i < roadSum; i++)
-        {
-            gpsTraceAvoid.clear();
-            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
-    }
-
-    if (lidarGridPtr!=NULL)
-    {
-        hasCheckedAvoidLidar = true;
-    }
-
-    for(int i=0; i<roadSum;i++){
-        std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;
-
-    }
-
-    checkAvoidObsTimes++;
-    if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
-    {
-        return - 1;
-    }
-
-
-    for (int i = 1; i < roadSum; i++)
-    {
-        if (roadNow + i < roadSum) {
-            avoidX = (roadOri-roadNow-i)*roadWidth;
-            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
-            {
-                /*if (roadNow==roadOri)
-                {
-                    avoidRunDistance = obsDisVector[roadNow] + Veh_Lenth;
-                    startAvoid_gps_ins = now_gps_ins;
-                }	*/
-                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + Veh_Lenth;
-                startAvoidGpsInsVector[roadNow] = now_gps_ins;
-                roadPre = roadNow + i;
-                return roadPre;
-            }
-        }
-
-        if (roadNow - i >= 0)
-        {
-            avoidX = (roadOri - roadNow+i)*roadWidth;
-            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
-            {
-                /*if (roadNow == roadOri)
-                {
-                    avoidRunDistance = obsDisVector[roadNow] + Veh_Lenth;
-                    startAvoid_gps_ins = now_gps_ins;
-                }*/
-                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + Veh_Lenth;
-                startAvoidGpsInsVector[roadNow] = now_gps_ins;
-                roadPre = roadNow - i;
-                return roadPre;
-            }
-        }
-
-    }
-    return roadPre;
-}
-
-int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
-    roadPre = -1;
-
-    computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-
-
-
-    if (roadNow>roadOri+1)
-    {
-        for (int i = roadOri+1; i < roadNow; i++)
-        {
-            gpsTraceBack.clear();
-            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-    }
-    else if (roadNow < roadOri - 1) {
-
-        for (int i = roadOri - 1; i > roadNow; i--)
-        {
-            gpsTraceBack.clear();
-            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-            avoidX = (roadWidth)*(roadOri - i);
-            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        }
-
-    }
-
-
-
-    if (lidarGridPtr != NULL)
-    {
-        hasCheckedBackLidar = true;
-    }
-
-
-
-    checkBackObsTimes++;
-    if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
-    {
-        return -1;
-    }
-
-
-
-
-
-
-
-    //if ((GetDistance(now_gps_ins, startAvoid_gps_ins)>avoidRunDistance)&&
-    //(checkReturnEnable(avoidX, lidarGridPtr,roadOri)))
-    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)) &&
-            (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
-    {
-        roadPre = roadOri;
-        return roadPre;
-    }
-
-    if (roadNow-roadOri>1)
-    {
-        for (int i = roadOri + 1;i < roadNow;i++) {
-            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
-                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  avoidMinDistanceVector[i])&&
-                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
-            {
-                roadPre = i;
-                return roadPre;
-            }
-        }
-    }
-
-    else if (roadNow <roadOri-1)
-    {
-        for (int i = roadOri - 1;i > roadNow;i--) {
-            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
-                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
-                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  30.0))
-            {
-                roadPre = i;
-                return roadPre;
-            }
-        }
-    }
-
-    return roadPre;
-}
-
-
-double iv::decition::DecideGps00::trumpet() {
-    if (trumpetFirstCount)
-    {
-        trumpetFirstCount = false;
-        trumpetLastTime= GetTickCount();
-        trumpetTimeSpan = 0.0;
-    }
-    else
-    {
-        trumpetStartTime= GetTickCount();
-        trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
-        trumpetLastTime = trumpetStartTime;
-    }
-
-    return trumpetTimeSpan;
-}
-
-
-
-
-double iv::decition::DecideGps00::transferP() {
-    if (transferFirstCount)
-    {
-        transferFirstCount = false;
-        transferLastTime= GetTickCount();
-        transferTimeSpan = 0.0;
-    }
-    else
-    {
-        transferStartTime= GetTickCount();
-        transferTimeSpan += transferStartTime - transferLastTime;
-        transferLastTime = transferStartTime;
-    }
-
-    return transferTimeSpan;
-}
-
-
-
-void  iv::decition::DecideGps00::handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins) {
-
-    if (abs(now_gps_ins.speed)>0.1)
-    {
-        decition->accelerator = 0;
-        decition->brake = 20;
-        decition->wheel_angle = 0;
-
-    }
-    else
-    {
-
-        decition->accelerator = 0;
-        decition->brake = 20;
-        decition->wheel_angle = 0;
-        handPark = true;
-        handParkTime = duringTime;
-    }
-
-}
-
-
-
-
-void iv::decition::DecideGps00::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins) {
-    gmapsL.clear();
-    gmapsR.clear();
-    for (int i = 0; i < 31; i++)
-    {
-        std::vector<iv::GPSData> gpsMapLineBeside;
-        //	gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
-        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);
-
-        gmapsL.push_back(gpsMapLineBeside);
-    }
-
-
-    for (int i = 0; i < 31; i++)
-    {
-        std::vector<iv::GPSData> gpsMapLineBeside;
-        //		gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
-        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);
-
-        gmapsR.push_back(gpsMapLineBeside);
-    }
-
-}
-
-
-bool iv::decition::DecideGps00::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {
-
-    if (lidarGridPtr == NULL)
-    {
-        return false;
-        //	lidarDistance = lastlidarDistance;
-
-    }
-    else {
-
-
-
-
-        obsPoint = Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
-        double lidarDistance = obsPoint.y - 2.5;   //激光距离推到车头
-        //     ODS("\n超车雷达距离:%f\n", lidarDistance);
-        if (lidarDistance >-20 && lidarDistance<35)
-        {
-            checkChaoCheBackCounts = 0;
-            return false;
-        }
-        else {
-            checkChaoCheBackCounts++;
-        }
-
-        if (checkChaoCheBackCounts>2) {
-            checkChaoCheBackCounts = 0;
-            return true;
-        }
-    }
-
-    return false;
-
-}
-
-void iv::decition::DecideGps00::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s){
-    Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);
-
-    ServiceCarStatus.group_x_local=pt.x;
-    //  ServiceCarStatus.group_y_local=pt.y;
-    ServiceCarStatus.group_y_local=s;
-    if(realspeed<0.36){
-        ServiceCarStatus.group_velx_local=0;
-        ServiceCarStatus.group_vely_local=0;
-    }else{
-        ServiceCarStatus.group_velx_local=realspeed*sin(theta)/3.6;
-        ServiceCarStatus.group_vely_local=realspeed*cos(theta)/3.6;
-    }
-
-
-    ServiceCarStatus.group_pathpoint=PathPoint;
-
-}
-
-
-
-float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
-                                                           int pathpoint,float secSpeed,float dSpeed){
-    float traffic_speed=200;
-    float traffic_dis=0;
-    float passTime;
-    float passSpeed;
-    bool passEnable=false;
-
-    if(abs(secSpeed)<0.1){
-        secSpeed=0;
-    }
-
-
-
-    if(pathpoint <= traffic_light_pathpoint){
-        for(int i=pathpoint;i<traffic_light_pathpoint;i++){
-            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
-        }
-    }else{
-        for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
-            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
-        }
-        for(int i=0;i<traffic_light_pathpoint;i++){
-            traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
-        }
-    }
-
-    //    if(traffic_light_color != 0)
-    //    {
-    //        int a = 3;
-    //    }
-
-
-    if(traffic_light_color==0 && traffic_dis<10){
-        traffic_speed=0;
-    }
-    //    else   //20200108
-    //    {
-    //        traffic_speed=10;
-    //    }
-    return  traffic_speed;
-
-    passSpeed = min((float)(dSpeed/3.6),secSpeed);
-    passTime = traffic_dis/(dSpeed/3.6);
-
-
-
-
-
-    switch(traffic_light_color){
-    case 0:
-        if(passTime>traffic_light_time+1 && traffic_dis>10){
-            passEnable=true;
-        }else{
-            passEnable=false;
-        }
-        break;
-    case 1:
-        if(passTime<traffic_light_time-1 && traffic_dis<10){
-            passEnable=true;
-        }else{
-            passEnable = false;
-        }
-        break;
-    case 2:
-        if(passTime<traffic_light_time){
-            passEnable= true;
-        }else{
-            passEnable=false;
-        }
-        break;
-
-
-    default:
-        break;
-    }
-
-    if(!passEnable){
-        if(traffic_dis<5){
-            traffic_speed=0;
-        }else if(traffic_dis<10){
-            traffic_speed=5;
-        }else if(traffic_dis<20){
-            traffic_speed=15;
-        }else if(traffic_dis<30){
-            traffic_speed=25;
-        }else if(traffic_dis<50){
-            traffic_speed=30;
-        }
-    }
-    return traffic_speed;
-
-}
-
-void iv::decition::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
-{
-    //    Point2D obsCombinePoint = Point2D(-1,-1);
-    iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
-    double obsSd;
-    if (lidarGridPtr == NULL)
-    {
-        lidarDistance = lastLidarDis;
-        //	lidarDistance = lastlidarDistance;
-    }
-    else {
-        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
-        //        lidarDistance = obsPoint.y-2.5;   //激光距离推到车头
-        iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
-        lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;
-
-        //    lidarDistance=-1;
-        if (lidarDistance<0)
-        {
-            lidarDistance = -1;
-        }
-        lastLidarDis = lidarDistance;
-    }
-
-    FrenetPoint esr_obs_frenet_point;
-    getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);
-
-    if(lidarDistance<0){
-        lidarDistance=500;
-    }
-    if(esrDistance<0){
-        esrDistance=500;
-    }
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
-    myesrDistance = esrDistance;
-
-    if(lidarDistance==500){
-        lidarDistance=-1;
-    }
-    if(esrDistance==500){
-        esrDistance=-1;
-    }
-
-    ServiceCarStatus.mRadarObs = esrDistance;
-    ServiceCarStatus.mLidarObs = lidarDistance;
-
-    //    //zhuanwan pingbi haomibo
-    //    if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
-    //        esrDistance=-1;
-    //    }
-
-    if (esrDistance>0 && lidarDistance > 0)
-    {
-        if (lidarDistance >= esrDistance)
-        {
-            obs = esrDistance;
-            //            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
-            obsSd = obsSpeed;
-            //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
-            //            obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
-        }
-        else if (!ServiceCarStatus.obs_radar.empty())
-        {
-            obs = lidarDistance;
-            //            obsCombinePoint = obsPoint;
-            //            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
-            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
-            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
-        }
-        else
-        {
-            obs=lidarDistance;
-            //            obsCombinePoint = obsPoint;
-            obsSd = 0 -secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
-            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
-        }
-    }
-    else if (esrDistance>0)
-    {
-        obs = esrDistance;
-        //        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
-        obsSd = obsSpeed;
-        //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
-        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
-    }
-    else if (lidarDistance > 0)
-    {
-        obs = lidarDistance;
-        //        obsCombinePoint = obsPoint;
-        obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
-        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
-    }
-    else {
-        obs = esrDistance;
-        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
-        obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
-    }
-
-    obsDistance=obs;
-    obsSpeed=obsSd;
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
-
-    ServiceCarStatus.mObs = obsDistance;
-    if(ServiceCarStatus.mObs>100){
-        ServiceCarStatus.mObs =-1;
-    }
-
-    if (obsDistance>0)
-    {
-        lastDistance = obsDistance;
-    }
-
-    if(obs<0){
-        obsDistance=500;
-    }else{
-        obsDistance=obs;
-    }
-}
-
-void iv::decition::DecideGps00::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
-
-
-    esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum);
-
-    if (esrIndex != -1)
-    {
-        esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
-        obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
-
-    }
-    else {
-        esrDistance = -1;
-    }
-}
-
-void iv::decition::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point, FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {
-
-    esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps);
-
-    if (esrIndex != -1)
-    {
-        //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
-        //严格来说应是 esrDistance=障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
-        esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset;  //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。
-
-        double speedx=ServiceCarStatus.obs_radar[esrIndex].speed_x;  //障碍物相对于车辆x轴的速度
-        double speedy=ServiceCarStatus.obs_radar[esrIndex].speed_y;  //障碍物相对于车辆y轴的速度
-        double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
-        //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
-        //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
-        double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);
-
-        obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
-    }
-    else {
-        esrDistance = -1;
-    }
-}
-
-void iv::decition::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine){
-    v2xTrafficVector.clear();
-    for (int var = 0; var < gpsMapLine.size(); var++) {
-        if(gpsMapLine[var]->roadMode==6){
-            v2xTrafficVector.push_back(var);
-        }
-    }
-}
-
-float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
-                                                             int pathpoint,float secSpeed,float dSpeed, bool circleMode){
-    float trafficSpeed=200;
-    int nearTraffixPoint=-1;
-    float nearTrafficDis=0;
-    int traffic_color=0;
-    int traffic_time=0;
-    bool passThrough=false;
-    float dSecSpeed=dSpeed/3.6;
-
-    if(v2xTrafficVector.empty()){
-        return trafficSpeed;
-    }
-    if(!circleMode){
-        if(pathpoint>v2xTrafficVector.back()){
-            return trafficSpeed;
-        }else {
-            for(int i=0; i< v2xTrafficVector.size();i++){
-                if (pathpoint<= v2xTrafficVector[i]){
-                    nearTraffixPoint=v2xTrafficVector[i];
-                    break;
-                }
-            }
-        }
-    }else if(circleMode){
-        if(pathpoint>v2xTrafficVector.back()){
-            nearTraffixPoint=v2xTrafficVector[0];
-        }else {
-            for(int i=0; i< v2xTrafficVector.size();i++){
-                if (pathpoint<= v2xTrafficVector[i]){
-                    nearTraffixPoint=v2xTrafficVector[i];
-                    break;
-                }
-            }
-        }
-    }
-
-
-    if(nearTraffixPoint!=-1){
-        for(int i=pathpoint;i<nearTraffixPoint;i++){
-            nearTrafficDis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
-        }
-    }
-
-    if(nearTrafficDis>50){
-        return trafficSpeed;
-    }
-
-
-    int roadMode = gpsMapLine[pathpoint]->roadMode;
-    if(roadMode==14 || roadMode==16){
-        traffic_color=trafficLight.leftColor;
-        traffic_time=trafficLight.leftTime;
-    }else if(roadMode==15 ||roadMode==17){
-        traffic_color=trafficLight.rightColor;
-        traffic_time=trafficLight.rightTime;
-    }else {
-        traffic_color=trafficLight.straightColor;
-        traffic_time=trafficLight.straightTime;
-    }
-
-
-    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
-    if(passThrough){
-        return trafficSpeed;
-    }else{
-        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
-        if(nearTrafficDis<6){
-            float decelerate =0-( secSpeed*secSpeed*0.5/nearTrafficDis);
-            minDecelerate=min(minDecelerate,decelerate);
-        }
-        return trafficSpeed;
-    }
-
-    return trafficSpeed;
-}
-
-bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed){
-    float passTime=0;
-    if (trafficColor==2 || trafficColor==3){
-        return false;
-    }else if(trafficColor==0){
-        return true;
-    }else{
-        passTime=trafficDis/dSecSpeed;
-        if(passTime+1< trafficTime){
-            return true;
-        }else{
-            return false;
-        }
-
-    }
-
-}
-
-float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
-    float limit=200;
-    if(trafficDis<10){
-        limit = 0;
-    }else if(trafficDis<15){
-        limit = 5;
-    }else if(trafficDis<20){
-        limit=10;
-    }else if(trafficDis<30){
-        limit=15;
-    }
-    return limit;
-}
-
-void iv::decition::DecideGps00::adjuseGpsLidarPosition(){
-
-    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
-    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
-
-}
+#include <decition/decide_gps_00.h>
+#include <decition/adc_tools/compute_00.h>
+#include <decition/adc_tools/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/obs_predict.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <control/can.h>
+#include <time.h>
+#include <decition/adc_controller/base_controller.h>
+#include <decition/adc_controller/pid_controller.h>
+#include <decition/adc_planner/lane_change_planner.h>
+#include <decition/adc_planner/frenet_planner.h>
+#include <QTime>
+
+using namespace std;
+
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+#define PI (3.1415926535897932384626433832795)
+
+iv::decition::DecideGps00::DecideGps00() {
+
+}
+iv::decition::DecideGps00::~DecideGps00() {
+
+}
+
+float iv::decition::DecideGps00::xiuzhengCs=0;
+
+int iv::decition::DecideGps00::PathPoint = -1;
+double iv::decition::DecideGps00::minDis = iv::MaxValue;
+double iv::decition::DecideGps00::maxAngle = iv::MinValue;
+//int iv::decition::DecideGps00::lastGpsIndex = iv::MaxValue;
+int iv::decition::DecideGps00::lastGpsIndex = 0;
+double iv::decition::DecideGps00::controlSpeed = 0;
+double iv::decition::DecideGps00::controlBrake = 0;
+double iv::decition::DecideGps00::controlAng = 0;
+double iv::decition::DecideGps00::Iv = 0;
+double iv::decition::DecideGps00::lastU = 0;
+double iv::decition::DecideGps00::lastVt = 0;
+double iv::decition::DecideGps00::lastEv = 0;
+
+int iv::decition::DecideGps00::gpsLineParkIndex = 0;
+int iv::decition::DecideGps00::gpsMapParkIndex = 0;
+double iv::decition::DecideGps00::lastDistance = MaxValue;
+double iv::decition::DecideGps00::lastPreObsDistance = MaxValue;
+double iv::decition::DecideGps00::obsDistance = -1;
+double iv::decition::DecideGps00::obsSpeed=0;
+double iv::decition::DecideGps00::obsDistanceAvoid = -1;
+double iv::decition::DecideGps00::lastDistanceAvoid = -1;
+
+double iv::decition::DecideGps00::lidarDistance = -1;
+double iv::decition::DecideGps00::myesrDistance = -1;
+double iv::decition::DecideGps00::lastLidarDis = -1;
+
+bool iv::decition::DecideGps00::parkMode = false;
+bool iv::decition::DecideGps00::readyParkMode = false;
+
+bool iv::decition::DecideGps00::zhucheMode = false;
+bool iv::decition::DecideGps00::readyZhucheMode = false;
+bool iv::decition::DecideGps00::hasZhuched = false;
+
+double iv::decition::DecideGps00::lastLidarDisAvoid = -1;
+double iv::decition::DecideGps00::lidarDistanceAvoid = -1;
+
+int iv::decition::DecideGps00::finishPointNum = -1;
+int iv::decition::DecideGps00::zhuchePointNum = 0;
+
+///kkcai, 2020-01-03
+bool iv::decition::DecideGps00::isFirstRun = true;
+//////////////////////////////////////////////
+
+float iv::decition::DecideGps00::minDecelerate=0;
+//bool iv::decition::DecideGps00::startingFlag = true;//起步标志，在起步时需要进行frenet规划。
+
+double offset_real = 0;
+double realspeed;
+double dSpeed;
+double dSecSpeed;
+double secSpeed;
+double ac;
+
+
+iv::Point2D obsPoint(-1, -1);
+iv::Point2D obsPointAvoid(-1, -1);
+
+
+int esrIndex = -1;
+int esrIndexAvoid = -1;
+double obsSpeedAvoid = 0;
+
+double esrDistance = -1;
+double esrDistanceAvoid = -1;
+int obsLostTime = 0;
+int obsLostTimeAvoid = 0;
+
+// double avoidTime = 0;
+
+
+double avoidX =0;
+float roadWidth = 3.5;
+int roadSum =10;
+int roadNow = 0;
+int roadOri =0;
+int roadPre = -1;
+int lastRoadOri = 0;
+
+int lightTimes = 0;
+
+
+int lastRoadNum=1;
+
+int stopCount = 0;
+
+int gpsMissCount = 0;
+
+bool rapidStop = false;
+
+bool hasTrumpeted = false;
+
+
+bool changeRoad=false;
+//实验手刹
+bool handFirst = true;
+double handTimeSpan = 0;
+double handStartTime = 0;
+double handLastTime = 0;
+bool handPark = false;
+long handParkTime=10000;
+
+//喇叭
+bool trumpetFirstCount=true;
+double trumpetTimeSpan = 0;
+double trumpetStartTime = 0;
+double trumpetLastTime = 0;
+
+//过渡
+bool transferFirstCount=true;
+double transferTimeSpan = 0;
+double transferStartTime = 0;
+double transferLastTime = 0;
+
+bool busMode = false;
+bool parkBesideRoad=false;
+
+bool chaocheMode = false;
+bool specialSignle = false;
+
+double offsetX = 0;
+
+double steerSpeed=9000;
+
+bool transferPieriod;
+bool transferPieriod2;
+double traceDevi;
+
+bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划。
+bool useFrenet = false;    //标志是否启用frenet算法避障
+bool useOldAvoid = true;   //标志是否用老方法避障
+
+enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
+                waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
+                dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState;
+
+
+std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
+std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear;
+
+std::vector<double> esrDisVector(roadSum, -1);
+std::vector<double> lidarDisVector(roadSum, -1);
+std::vector<double> obsDisVector(roadSum,-1);
+std::vector<double> obsSpeedVector(roadSum, 0);
+
+std::vector<int> obsLostTimeVector(roadSum, 0);
+
+std::vector<double> lastLidarDisVector(roadSum, -1);
+std::vector<double> lastDistanceVector(roadSum, -1);
+
+bool qiedianCount = false;
+//日常展示
+
+iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
+                                                                   const std::vector<GPSData> gpsMapLine,
+                                                                   iv::LidarGridPtr lidarGridPtr,
+                                                                   std::vector<iv::Perception::PerceptionOutput> lidar_per,
+                                                                   iv::TrafficLight trafficLight)
+{
+    //   QTime xTime;
+    //    xTime.start();
+    //39.14034649083606 117.0863975920476 20507469.630314853  4334165.046101382 353
+    Decition gps_decition(new DecitionBasic);
+    //    vector<iv::Point2D> fpTraceTmp;
+
+
+    //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
+    if(!(useFrenet^useOldAvoid)){
+        useFrenet = false;
+        useOldAvoid = true;
+    }
+
+    //    //如果useFrenet、useOldAvoid两者均为真或均为假，则采用原来的方法
+    //    if(useFrenet&&useOldAvoid || !useFrenet&&!useOldAvoid){
+    //        useFrenet = false;
+    //        useOldAvoid = true;
+    //    }
+
+    //  ServiceCarStatus.group_control=false;
+
+
+
+
+//    GPS_INS gps= Coordinate_UnTransfer(0,1.5,now_gps_ins);
+//    now_gps_ins.gps_x=gps.gps_x;
+//    now_gps_ins.gps_y=gps.gps_y;
+
+    //    GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
+
+
+    if (isFirstRun)
+    {
+        initMethods();
+        vehState = normalRun;
+        startAvoid_gps_ins = now_gps_ins;
+        init();
+        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
+                                                          DecideGps00::lastGpsIndex,
+                                                          DecideGps00::minDis,
+                                                          DecideGps00::maxAngle);
+        DecideGps00::lastGpsIndex = PathPoint;
+
+        adjuseGpsLidarPosition();
+        if(ServiceCarStatus.speed_control==true){
+            Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
+        }
+
+        //	ServiceCarStatus.carState = 1;
+        //        roadOri = gpsMapLine[PathPoint]->roadOri;
+        //        roadNow = roadOri;
+        //        roadSum = gpsMapLine[PathPoint]->roadSum;
+        //  busMode = false;
+        //  vehState = dRever;
+
+        double dis =  GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
+        if(dis<15){
+            circleMode=true;  //201200102
+        }else{
+            circleMode=false;
+        }
+        //     circleMode = true;
+
+
+        getV2XTrafficPositionVector(gpsMapLine);
+        //        group_ori_gps=*gpsMapLine[0];
+        ServiceCarStatus.bocheMode=false;
+        ServiceCarStatus.daocheMode=false;
+        parkMode=false;
+        readyParkMode=false;
+        finishPointNum=-1;
+        isFirstRun = false;
+    }
+
+
+    if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
+        GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
+        now_gps_ins.gps_x=gpsOffset.gps_x;
+        now_gps_ins.gps_y=gpsOffset.gps_y;
+        GaussProjInvCal(now_gps_ins.gps_x,  now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
+    }
+
+
+
+    //1227
+    //  ServiceCarStatus.daocheMode=true;
+
+    //1220
+    changingDangWei=false;
+
+    minDecelerate=0;
+    if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90){
+        //  int a=0;
+        gps_decition->wheel_angle = 0;
+        gps_decition->speed = dSpeed;
+
+        gps_decition->accelerator = -0.5;
+        gps_decition->brake=10;
+        return gps_decition;
+    }
+
+
+
+
+
+    //1220
+
+
+    if(ServiceCarStatus.daocheMode){
+
+        now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
+        if( now_gps_ins.ins_heading_angle>=360)
+            now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
+    }
+    //1220 end
+
+
+
+    //1125 traficc
+    traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
+    traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
+    GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
+
+
+    //xiesi
+    ///kkcai, 2020-01-03
+    //ServiceCarStatus.busmode=true;
+    //ServiceCarStatus.busmode=false;//20200102
+    ///////////////////////////////////////////////////
+
+
+    busMode = ServiceCarStatus.busmode;
+    nearStation=false;
+
+    gps_decition->leftlamp = false;
+    gps_decition->rightlamp = false;
+    //    qDebug("heading is %g",now_gps_ins.ins_heading_angle);
+
+
+    aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
+    aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;
+
+    aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;
+
+
+
+
+    GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+    is_arrivaled = false;
+
+
+
+
+    //  xiuzhengCs=-0.8;  //1026
+
+    xiuzhengCs=0;
+    //    if (parkMode)
+    //    {
+
+
+    //        handBrakePark(gps_decition,10000,now_gps_ins);
+    //        return gps_decition;
+    //    }
+
+
+
+
+
+    realspeed = now_gps_ins.speed;
+
+    secSpeed = realspeed / 3.6;
+
+
+
+
+    //sidePark
+
+    if(ServiceCarStatus.mnParkType==1){
+
+        if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
+            int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+            if(bocheEN==1){
+                ServiceCarStatus.bocheEnable=1;
+            }else if(bocheEN==0){
+                ServiceCarStatus.bocheEnable=0;
+            }
+        }else{
+            ServiceCarStatus.bocheEnable=2;
+        }
+
+
+
+        if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&
+                vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4&&  vehState!=reverseArr ){
+            if(abs(realspeed)>0.1){
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }else{
+                if(trumpet()>3000){
+                    trumpetFirstCount=true;
+                    vehState=dRever;
+                }
+
+            }
+            //         ServiceCarStatus.bocheMode=false;
+        }
+
+    }
+
+
+    //chuizhiPark
+
+    if(ServiceCarStatus.mnParkType==0){
+        if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr ){
+            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+            if(bocheEN==1){
+                ServiceCarStatus.bocheEnable=1;
+            }else if(bocheEN==0){
+                ServiceCarStatus.bocheEnable=0;
+            }
+        }else{
+            ServiceCarStatus.bocheEnable=2;
+        }
+
+
+
+        if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect&&  vehState!=reverseArr ){
+
+            if(abs(realspeed)>0.1){
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }else{
+                if(trumpet()>3000){
+                    trumpetFirstCount=true;
+                    vehState=reverseCar;
+                }
+
+            }
+            //     ServiceCarStatus.bocheMode=false;
+
+        }
+    }
+    // ChuiZhiTingChe
+    if (vehState == reverseCar)
+    {
+        Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+
+        vehState = reversing;
+        qiedianCount=true;
+
+
+    }
+    if (vehState == reversing)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
+        if (dis<2.0)//0.5
+        {
+            vehState = reverseCircle;
+            qiedianCount = true;
+            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
+        }
+        else {
+            controlAng = 0;
+            dSpeed = 2;
+            dSecSpeed = dSpeed / 3.6;
+            gps_decition->wheel_angle = 0;
+            gps_decition->speed = dSpeed;
+            obsDistance=-1;
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            //	gps_decition->accelerator = 0;
+            return gps_decition;
+        }
+    }
+
+    if (vehState == reverseCircle)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
+        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
+        if((((angdis<5)||(angdis>355)))&&(dis<3.0))
+            //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
+        {
+            vehState = reverseDirect;
+            qiedianCount = true;
+            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+
+        }
+        else {
+            if (qiedianCount && trumpet()<0)
+                //         if (qiedianCount && trumpet()<1500)
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+
+
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            }
+
+            controlAng = Compute00().bocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng*1.05;
+
+            cout<<"farTpointDistance================"<<dis<<endl;
+
+            return gps_decition;
+        }
+    }
+
+    if (vehState == reverseDirect)
+    {
+        //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
+        Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+
+        if ((pt.y)<0.5)
+        {
+
+            qiedianCount=true;
+            vehState=reverseArr;
+            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
+            //            gps_decition->accelerator = -3;
+            //            gps_decition->brake = 10;
+            //            gps_decition->torque = 0;
+            gps_decition->wheel_angle=0;
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            return gps_decition;
+
+
+
+
+        }
+        else {
+
+            //if (qiedianCount && trumpet()<0)
+            if (qiedianCount && trumpet()<1000)
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+                dSpeed = 1;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+
+
+
+            controlAng = 0;
+
+            gps_decition->wheel_angle = 0;
+
+            return gps_decition;
+        }
+    }
+
+
+
+    if (vehState == reverseArr)
+    {
+        //
+
+        ServiceCarStatus.bocheMode=false;
+        if (qiedianCount && trumpet()<1500)
+        {
+
+            //            gps_decition->brake = 10;
+            //            gps_decition->torque = 0;
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            ServiceCarStatus.bocheMode=false;
+        }
+        else
+        {
+            qiedianCount = false;
+            trumpetFirstCount = true;
+            ServiceCarStatus.bocheEnable=0;
+            vehState=normalRun;
+            ServiceCarStatus.mbRunPause=true;
+            ServiceCarStatus.mnBocheComplete=100;
+            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
+        }
+
+
+        gps_decition->wheel_angle = 0 ;
+
+
+
+        return gps_decition;
+
+    }
+
+
+
+    //ceFangWeiBoChe
+
+    if (vehState == dRever)
+    {
+        GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+
+        Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+
+        vehState = dRever0;
+        qiedianCount=true;
+
+
+        std::cout<<"enter side boche mode"<<std::endl;
+
+
+
+    }
+    if (vehState == dRever0)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
+        if (dis<2.0)
+        {
+            vehState = dRever1;
+            qiedianCount = true;
+            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
+        }
+        else {
+            controlAng = 0;
+            dSpeed = 2;
+            dSecSpeed = dSpeed / 3.6;
+            gps_decition->wheel_angle = 0;
+            gps_decition->speed = dSpeed;
+            obsDistance=-1;
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            //	gps_decition->accelerator = 0;
+            return gps_decition;
+        }
+    }
+
+
+    if (vehState == dRever1)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);
+
+        if(dis<2.0)
+        {
+            vehState = dRever2;
+            qiedianCount = true;
+            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+
+        }
+        else {
+            if (qiedianCount && trumpet()<1000)
+
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+
+
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            }
+
+            controlAng = Compute00().dBocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng;
+
+            cout<<"farTpointDistance================"<<dis<<endl;
+
+            return gps_decition;
+        }
+    }
+
+
+    if (vehState == dRever2)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
+        Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+        Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
+        double xx= (pt1.x-pt2.x);
+        // if(xx>0)
+        if(xx>-0.5)
+        {
+            GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+            Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+            double xxx=ptt.x;
+            double yyy =ptt.y;
+            //            if(xxx<-1.5||xx>1){
+            //                int a=0;
+            //            }
+            vehState = dRever3;
+            qiedianCount = true;
+            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+            cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
+            cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;
+
+
+        }
+        else {
+            if (qiedianCount && trumpet()<1000)
+
+            {
+
+                /*  gps_decition->brake = 10;
+                gps_decition->torque = 0;  */
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+
+
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            }
+
+
+            gps_decition->wheel_angle = 0 ;
+
+            cout<<"farTpointDistance================"<<dis<<endl;
+
+            return gps_decition;
+        }
+    }
+
+
+    if (vehState == dRever3)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
+        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
+        if((((angdis<5)||(angdis>355)))&&(dis<10.0))
+        {
+            vehState = dRever4;
+            qiedianCount = true;
+            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+
+        }
+        else {
+            if (qiedianCount && trumpet()<1000)
+
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+
+
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            }
+
+            controlAng = 0-Compute00().dBocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng*0.95;
+
+            cout<<"farTpointDistance================"<<dis<<endl;
+
+            return gps_decition;
+        }
+    }
+
+
+
+    if (vehState == dRever4)
+    {
+        //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
+        Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+
+        if ((pt.y)<0.5)
+        {
+
+            qiedianCount=true;
+            vehState=reverseArr;
+            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
+            //            gps_decition->accelerator = -3;
+            //            gps_decition->brake =10 ;
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            gps_decition->wheel_angle=0;
+            return gps_decition;
+
+
+
+
+        }
+        else {
+
+            //if (qiedianCount && trumpet()<0)
+            if (qiedianCount && trumpet()<1000)
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+
+
+
+            controlAng = 0;
+
+            gps_decition->wheel_angle = 0;
+
+            return gps_decition;
+        }
+    }
+
+
+
+    if (vehState == reverseArr)
+    {
+        //
+        ServiceCarStatus.bocheMode=false;
+
+        if (qiedianCount && trumpet()<1500)
+        {
+
+            //            gps_decition->brake = 10;
+            //            gps_decition->torque = 0;
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+        }
+        else
+        {
+            qiedianCount = false;
+            trumpetFirstCount = true;
+            ServiceCarStatus.bocheEnable=0;
+            vehState=normalRun;
+            ServiceCarStatus.mbRunPause=true;
+            ServiceCarStatus.mnBocheComplete=100;
+            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
+        }
+
+
+        gps_decition->wheel_angle = 0 ;
+
+
+
+        return gps_decition;
+
+    }
+
+
+
+
+
+
+
+
+
+
+
+    obsDistance = -1;
+    esrIndex = -1;
+    lidarDistance = -1;
+
+    obsDistanceAvoid = -1;
+    esrIndexAvoid = -1;
+    roadPre = -1;
+
+
+
+
+
+
+
+
+
+    avoidX = (roadOri-roadNow)*roadWidth;
+
+    gpsTraceOri.clear();
+    gpsTraceNow.clear();
+    gpsTraceAim.clear();
+    gpsTraceAvoid.clear();
+    gpsTraceBack.clear();
+
+
+
+
+
+
+
+    if (!isFirstRun)
+    {
+        //   PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+        //    if(PathPoint<0){
+        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+        //    }
+
+    }
+
+
+    if (PathPoint<0)
+    {
+
+        minDecelerate=-1.0;
+        phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
+        return gps_decition;
+
+    }
+
+    DecideGps00::lastGpsIndex = PathPoint;
+    //2020-01-03, kkcai
+    //if(!circleMode && PathPoint>gpsMapLine.size()-200){
+    if(!circleMode && PathPoint>gpsMapLine.size()-100){
+        minDecelerate=-1.0;
+        std::cout<<"map finish brake"<<std::endl;
+    }
+
+
+
+
+    roadOri = gpsMapLine[PathPoint]->roadOri;
+
+    roadSum = gpsMapLine[PathPoint]->roadSum;
+
+
+    if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
+                                                ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
+    {
+        vehState = normalRun;
+        roadNow = roadOri;
+    }
+
+
+
+    //    if (PathPoint!=-1&&((now_gps_ins.rtk_status==6)||(now_gps_ins.rtk_status==5))&&GetDistance(now_gps_ins,*gpsMapLine[PathPoint])<30)
+    //    {
+    //        DecideGps00::lastGpsIndex = PathPoint;
+    //        gpsMissCount = 0;
+
+    //    }
+    //    else
+    //    {
+    //        gpsMissCount++;
+    //    }
+
+    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
+    //    gpsTraceOri = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
+    if(gpsTraceOri.empty()){
+        gps_decition->wheel_angle = 0;
+        gps_decition->speed = dSpeed;
+
+        gps_decition->accelerator = -0.5;
+        gps_decition->brake=10;
+        return gps_decition;
+    }
+
+
+
+    traceDevi=gpsTraceOri[0].x;
+
+    //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
+    //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
+    //    if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
+    //        startingFlag = false;
+    //    }
+    startingFlag = false;
+    if(startingFlag){
+        //        gpsTraceAim = gpsTraceOri;
+        if(abs(gpsTraceOri[0].x)>1){
+            cout<<"frenetPlanner->getPath:pre"<<endl;
+            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
+            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
+            if(gpsTraceNow.size()==0){
+                gps_decition->accelerator = 0;
+                gps_decition->brake=10;
+                gps_decition->wheel_angle = lastAngle;
+                gps_decition->speed = 0;
+                return gps_decition;
+            }
+        }else{
+            startingFlag = false;
+        }
+    }
+
+
+    if(useFrenet){
+        //如果车辆在变道中，启用frenet规划局部路径
+        if(vehState == avoiding || vehState == backOri){
+            avoidX = (roadOri - roadNow)*roadWidth;
+            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
+            if(gpsTraceNow.size()==0){
+                gps_decition->accelerator = 0;
+                gps_decition->brake=10;
+                gps_decition->wheel_angle = lastAngle;
+                gps_decition->speed = 0;
+                return gps_decition;
+            }
+        }
+    }
+
+    if(gpsTraceNow.size()==0){
+        if (roadNow==roadOri)
+        {
+            gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+            //gpsTraceNow = getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex);
+        }else
+        {
+            //	gpsTraceNow = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-roadNow))], lastGpsIndex);
+            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+            //gpsTraceNow = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+        }
+    }
+
+
+    //  dSpeed = getSpeed(gpsTraceNow);
+    dSpeed = 80;
+
+    planTrace = gpsTraceNow; //Add By YuChuli, 2020.11.26
+
+    dSpeed = limitSpeed(controlAng, dSpeed);
+
+
+
+    controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
+    if(!circleMode && PathPoint>(gpsMapLine.size()-100)){
+        controlAng=0;
+    }
+
+
+
+    //1220
+    if(ServiceCarStatus.daocheMode){
+        controlAng=0-controlAng;
+    }
+
+    //2020-0106
+    if(vehState==avoiding){
+        controlAng=max(-150.0,controlAng);
+        controlAng=min(150.0,controlAng);
+    }
+    if(vehState==backOri){
+        controlAng=max(-120.0,controlAng);
+        controlAng=min(120.0,controlAng);
+    }
+
+    //准备驻车
+
+    if (readyZhucheMode)
+    {
+
+        cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
+        cout<<"zhuche point : "<<zhuchePointNum<<endl;
+
+        double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
+
+        if (dis<35)
+        {
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
+            double zhucheDistance = pt.y;
+#if 0
+
+            if (dSpeed > 15)
+            {
+                dSpeed = 15;
+            }
+
+
+
+            if (zhucheDistance < 20 && dis < 25)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+#else
+            if (dSpeed > 12)
+            {
+                dSpeed = 12;
+            }
+
+
+
+            if (zhucheDistance < 9 && dis < 9)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+#endif
+
+
+            if (zhucheDistance < 3.0 && dis < 3.5)
+            {
+                dSpeed = min(dSpeed, 5.0);
+                zhucheMode = true;
+                readyZhucheMode = false;
+                cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
+
+                //1125
+                //                 gps_decition->brake=20;
+                //                 gps_decition->accelerator = -3;
+                //                 gps_decition->wheel_angle = 0-controlAng;
+                //                 gps_decition->speed = 0;
+                //                 gps_decition->torque=0;
+                //                 return gps_decition;
+
+
+
+
+
+            }
+
+        }
+
+    }
+
+
+
+
+
+    if (readyParkMode)
+    {
+        double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
+        hasZhuched = true;
+
+        if (parkDis < 25)
+        {
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
+            double parkDistance = pt.y;
+
+
+            if (dSpeed > 15)
+            {
+                dSpeed = 15;
+            }
+
+            //dSpeed = 15;//////////////////////////////
+
+            if (parkDistance < 15 && parkDistance >= 4.5 && parkDis<20)
+            {
+                dSpeed = min(dSpeed, 8.0);
+            }else if (parkDistance < 4.5 && parkDistance >= 3.5 && parkDis<5.0){
+                dSpeed = min(dSpeed, 5.0);
+            }else if(parkDistance < 3.5 && parkDis<4.0){
+                dSpeed = min(dSpeed, 3.0);
+            }
+
+
+            //            float stopDis=2;
+            //            if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
+            //                stopDis=1.6;
+            //            } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
+            //                stopDis=1.8;
+            //            }
+
+            if (parkDistance < 1.6  && parkDis<2.0)
+            {
+                // dSpeed = 0;
+                parkMode = true;
+                readyParkMode = false;
+            }
+
+
+        }
+    }
+
+
+
+    if (gpsMapLine[PathPoint]->roadMode ==0)
+    {
+        //dSpeed = min(10.0,dSpeed);
+
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+        gps_decition->leftlamp = false;
+        gps_decition->rightlamp = false;
+
+    }else  if (gpsMapLine[PathPoint]->roadMode == 5)
+    {
+        //dSpeed = min(8.0,dSpeed);
+
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
+        gps_decition->leftlamp = false;
+        gps_decition->rightlamp = false;
+
+    }else if (gpsMapLine[PathPoint]->roadMode == 11)
+    {
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
+
+    }else if (gpsMapLine[PathPoint]->roadMode == 14)
+    {
+        //dSpeed = min(8.0,dSpeed);
+
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
+        gps_decition->leftlamp = true;
+        gps_decition->rightlamp = false;
+    }else if (gpsMapLine[PathPoint]->roadMode == 15)
+    {
+        //dSpeed = min(8.0,dSpeed);
+
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
+        gps_decition->leftlamp = false;
+        gps_decition->rightlamp = true;
+    }else if (gpsMapLine[PathPoint]->roadMode == 16)
+    {
+        // dSpeed = min(10.0,dSpeed);
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
+        gps_decition->leftlamp = true;
+        gps_decition->rightlamp = false;
+    }else if (gpsMapLine[PathPoint]->roadMode == 17)
+    {
+        //  dSpeed = min(10.0,dSpeed);
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
+        gps_decition->leftlamp = false;
+        gps_decition->rightlamp = true;
+    }else if (gpsMapLine[PathPoint]->roadMode == 18)
+    {
+        // dSpeed = min(10.0,dSpeed);
+        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
+        /*if(gpsMapLine[PathPoint]->light_left_or_right == 1)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+        else if(gpsMapLine[PathPoint]->light_left_or_right == 2)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }*/
+    }else if (gpsMapLine[PathPoint]->roadMode == 1)
+    {
+        dSpeed = min(10.0,dSpeed);
+    }else if (gpsMapLine[PathPoint]->roadMode == 2)
+    {
+        dSpeed = min(15.0,dSpeed);
+    }
+    else if (gpsMapLine[PathPoint]->roadMode == 7)
+    {
+        dSpeed = min(15.0,dSpeed);
+        xiuzhengCs=1.5;
+    }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
+    {
+        dSpeed = min(4.0,dSpeed);
+
+    }
+
+
+
+
+
+    if (gpsMapLine[PathPoint]->speed_mode == 2)
+    {
+        dSpeed = min(25.0,dSpeed);
+
+    }
+    dSecSpeed = dSpeed / 3.6;
+
+
+
+
+
+
+    std::cout<<"juecesudu2="<<dSpeed<<std::endl;
+
+
+
+
+
+
+
+    if(vehState==changingRoad){
+        if(gpsTraceNow[0].x>1.0){
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }else if(gpsTraceNow[0].x<-1.0){
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }else{
+            vehState==normalRun;
+        }
+    }
+
+    //    qDebug("decide0 time is %d",xTime.elapsed());
+
+    //1220
+    if(!ServiceCarStatus.daocheMode){
+        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+    }else{
+        gpsTraceRear.clear();
+        for(int i=0;i<gpsTraceNow.size();i++){
+            Point2D pt;
+            pt.x=0-gpsTraceNow[i].x;
+            pt.y=0-gpsTraceNow[i].y;
+            gpsTraceRear.push_back(pt);
+        }
+        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        //  obsDistance=-1; //1227
+    }
+
+
+
+
+    //old_bz--------------------------------------------------------------------------------------------------
+
+    if (vehState == avoiding)
+    {
+        cout<<"\n==================avoiding=================\n"<<endl;
+        //  vehState=normalRun; //1025
+        if (dSpeed > 10) {
+            dSpeed = 10;
+        }
+        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
+        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
+            vehState = normalRun;
+            //            useFrenet = false;
+            //            useOldAvoid = true;
+        }
+        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
+            // vehState = avoidObs; 0929
+            vehState = normalRun;
+        }
+        else if (gpsTraceNow[0].x>0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if(gpsTraceNow[0].x<0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+
+    if (vehState==preBack)
+    {
+        vehState = backOri;
+    }
+
+
+    if (vehState==backOri)
+    {
+        cout<<"\n================backOri===========\n"<<endl;
+        //  vehState=normalRun; //1025
+        if (dSpeed > 10) {
+            dSpeed = 10;
+        }
+
+        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
+        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
+            vehState = normalRun;
+            //            useFrenet = false;
+            //            useOldAvoid = true;
+        }
+        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
+            // vehState = avoidObs; 0929
+            vehState = normalRun;
+        }
+        else if (gpsTraceNow[0].x>0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if (gpsTraceNow[0].x<0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+    std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
+
+    cout<<"\n当前RoadNum:%d\n"<<roadNow<<endl;
+
+
+
+
+
+
+
+
+
+    //   计算回归原始路线
+
+    if (roadNow!=roadOri)
+    {
+        //        useFrenet = true;
+        //        useOldAvoid = false;
+
+        if(useFrenet){
+            if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
+            {
+                double offsetL = -(roadSum - 1)*roadWidth;
+                double offsetR = (roadOri - 0)*roadWidth;
+                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
+            }
+        }
+        else if(useOldAvoid){
+            roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
+            avoidX = (roadOri - roadNow)*roadWidth; //1012
+        }
+    }
+    if (roadNow != roadOri && roadPre!=-1)
+    {
+
+        roadNow = roadPre;
+        avoidX = (roadOri - roadNow)*roadWidth;
+        gpsTraceNow.clear();
+        if(useOldAvoid){
+            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+        }
+        else if(useFrenet){
+            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+        }
+
+        vehState = backOri;
+        hasCheckedBackLidar = false;
+        //  checkBackObsTimes = 0;
+
+        cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
+
+    }
+
+    //shiyanbizhang 0929
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.socfDis+20.0) &&(abs(realspeed)<1.0) &&
+            (vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
+            && (gpsMapLine[PathPoint]->runMode==1)){
+        ObsTimeStart=GetTickCount();
+        cout<<"\n====================preAvoid time count start==================\n"<<endl;
+    }
+    if(ObsTimeStart!=-1){
+        ObsTimeEnd=GetTickCount();
+    }
+    if(ObsTimeEnd!=-1){
+        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
+    }
+    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+        vehState=avoidObs;
+        ObsTimeStart=-1;
+        ObsTimeEnd=-1;
+        ObsTimeSpan=-1;
+        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
+    }
+
+    if(obsDistance<0 ||obsDistance>ServiceCarStatus.socfDis){
+        ObsTimeStart=-1;
+        ObsTimeEnd=-1;
+        ObsTimeSpan=-1;
+
+    }
+
+
+    //避障模式
+
+
+    if (vehState == avoidObs   || vehState==waitAvoid ) {
+
+        //      if (obsDistance <20 && obsDistance >0)
+        if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
+        {
+            dSpeed = min(7.0,dSpeed);
+            avoidTimes++;
+            //          if (avoidTimes>=6)
+            if (avoidTimes>=ServiceCarStatus.aocfTimes)
+            {
+                vehState = preAvoid;
+                avoidTimes = 0;
+            }
+
+        }
+        //        else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
+        //        {
+        //            dSpeed = 10;
+        //            avoidTimes = 0;
+        //        }
+        else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
+        {
+            dSpeed =  min(15.0,dSpeed);
+            avoidTimes = 0;
+        }
+        else
+        {
+            avoidTimes = 0;
+        }
+
+    }
+
+
+    if (vehState == preAvoid)
+    {
+        cout<<"\n====================preAvoid==================\n"<<endl;
+        /* if (obsDisVector[roadNow]>30)*/  //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
+        if (obsDisVector[roadNow]>ServiceCarStatus.aocfDis)
+        {
+            // vehState = avoidObs; 0929
+            vehState=normalRun;
+        }
+        else
+        {
+            //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
+            if(useOldAvoid){
+                roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
+                avoidX = (roadOri - roadNow)*roadWidth;  //1012
+            }
+            else if(useFrenet){
+                double offsetL = -(roadSum - 1)*roadWidth;
+                double offsetR = (roadOri - 0)*roadWidth;
+                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
+            }
+
+            if (roadPre == -1 && obsDisVector[roadNow]<1.5 && obsDisVector[roadNow]>0)
+            {
+                //  vehState = waitAvoid; 0929
+                vehState = avoidObs;
+            }
+            else if (roadPre != -1)
+            {
+                if(useOldAvoid){
+                    roadNow = roadPre;
+                    avoidX = (roadOri - roadNow)*roadWidth;
+                    gpsTraceNow.clear();
+                    gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+                }
+                else if(useFrenet){
+                    if(roadPre != roadNow){
+                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + Veh_Lenth;
+                        startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                    }
+                    roadNow = roadPre;
+                    avoidX = (roadOri - roadNow)*roadWidth;
+                    gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+                }
+
+
+                vehState = avoiding;
+
+                hasCheckedAvoidLidar = false;
+
+                cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;
+
+            }
+        }
+    }
+
+    //--------------------------------------------------------------------------------old_bz_end
+
+
+
+
+
+    //TOUCHEPINGBI
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    if (parkMode)
+    {
+        minDecelerate=-1.0;
+
+        if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
+            parkMode=false;
+        }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>20){
+            parkMode=false;
+        }
+
+    }
+
+
+
+    //驻车
+
+    if (zhucheMode)
+    {
+        int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;
+
+        //        if(trumpet()<16000)
+        if (trumpet()<mzcTime)
+        {
+            //            if (trumpet()<12000){
+            cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
+            minDecelerate=-1.0;
+            if(abs(realspeed)<0.2){
+                controlAng=0;  //tju
+            }
+        }
+        else
+        {
+            hasZhuched = true; //1125
+            zhucheMode = false;
+            trumpetFirstCount = true;
+            cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
+        }
+
+    }
+
+
+    //判断驻车标志位
+    if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
+    {
+        hasZhuched = false;
+    }
+
+
+
+
+
+
+
+
+
+
+
+    if ( vehState==changingRoad || vehState==chaocheBack)
+    {
+        double lastAng = 0.0 - lastAngle;
+        if (controlAng>40)
+        {
+            controlAng =40;
+        }
+        else if (controlAng<-40)
+        {
+            controlAng = - 40;
+        }
+
+
+    }
+
+
+    //速度结合角度的限制
+    controlAng = limitAngle(realspeed, controlAng);
+
+
+    std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;
+
+
+
+    //1220
+    if(PathPoint+80<gpsMapLine.size()-1){
+        if(gpsMapLine[PathPoint+80]->roadMode==4  && !ServiceCarStatus.daocheMode){
+            changingDangWei=true;
+        }
+    }
+
+    if(changingDangWei){
+        if(abs(realspeed)>0.1){
+            dSpeed=0;
+        }else{
+            dSpeed=0;
+            gps_decition->dangWei=2;
+            ServiceCarStatus.daocheMode=true;
+        }
+    }
+    //1220 end
+
+
+
+
+    for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
+    {
+        GPS_INS gpsIns;
+        GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude,  &gpsIns.gps_x, &gpsIns.gps_y);
+
+        double dis = GetDistance(gpsIns, now_gps_ins);
+        if(dis<20)
+            ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
+    }
+
+
+
+
+    if (ServiceCarStatus.carState == 0 && busMode)
+    {
+
+        minDecelerate=-1.0;
+    }
+
+
+
+    if (ServiceCarStatus.carState == 3 && busMode)
+    {
+
+        if(realspeed<0.1){
+            ServiceCarStatus.carState=0;
+            minDecelerate=-1.0;
+        }else{
+            nearStation=true;
+            dSpeed=0;
+        }
+
+    }
+
+
+
+
+
+
+
+    ///kkcai, 2020-01-03
+    //if (ServiceCarStatus.carState == 2 && busMode)
+    if (ServiceCarStatus.carState == 2)
+    {
+
+        aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
+        aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
+        //   gps2xy(ServiceCarStatus.amilat, ServiceCarStatus.amilng, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+        std::cout<<"lng"<<ServiceCarStatus.amilng<<std::endl;
+        qDebug("lat:%f", aim_gps_ins.gps_lat);
+        qDebug("lon:%f", aim_gps_ins.gps_lng);
+
+        std::cout<<"lat"<<ServiceCarStatus.amilat<<std::endl;
+        double dis = GetDistance(aim_gps_ins, now_gps_ins);
+        Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
+
+        std::cout << "\n呼车dis距离:%f\n" << dis << std::endl;
+        std::cout << "\n呼车Y距离:%f\n" << pt.y << std::endl;
+
+        //         if (dis<20 && pt.y<8&& realspeed<0.1)
+        if (dis<20 && pt.y<5 && abs(pt.x)<3)
+        {
+            dSpeed = 0;
+            nearStation=true;
+            //is_arrivaled = true;
+            //ServiceCarStatus.status[0] = true;
+            ServiceCarStatus.istostation=1;
+            minDecelerate=-1.0;
+        }
+
+        else if (dis<20 && pt.y<15 && abs(pt.x)<3)
+        {
+            nearStation=true;
+            dSpeed = min(8.0, dSpeed);
+        }
+        else if (dis<30 && pt.y<20 && abs(pt.x)<3)
+        {
+            dSpeed = min(15.0, dSpeed);
+        }
+
+        else if (dis<50 && abs(pt.x)<3)
+        {
+            dSpeed = min(20.0, dSpeed);
+        }
+    }
+
+
+    dSecSpeed = dSpeed / 3.6;
+    gps_decition->speed = dSpeed;
+
+
+
+    if (gpsMapLine[PathPoint]->runMode == 2)
+    {
+        obsDistance = -1;
+
+    }
+
+    std::cout<<"juecesudu0="<<dSpeed<<std::endl;
+
+    //-------------------------------traffic light----------------------------------------------------------------------------------------
+
+    if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
+        traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
+        //    traffic_light_pathpoint=130;
+        float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
+                                                     traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
+        dSpeed = min((double)traffic_speed,dSpeed);
+        if(traffic_speed==0){
+            minDecelerate=-2.0;
+        }
+    }
+    //-------------------------------traffic light  end-----------------------------------------------------------------------------------------------
+
+
+    //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------
+
+
+    double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight,  gpsMapLine, v2xTrafficVector,
+                                                         PathPoint, secSpeed, dSpeed,  circleMode);
+
+
+    dSpeed = min(v2xTrafficSpeed,dSpeed);
+
+    //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
+
+
+    if(obsDistance>0 && obsDistance<10){
+        dSpeed=0;
+    }
+    //  obsDistance=-1; //1227
+
+    if(ServiceCarStatus.group_control){
+        dSpeed = ServiceCarStatus.group_comm_speed;
+    }
+    if(dSpeed==0){
+        minDecelerate=min(-1.0f,minDecelerate);
+    }
+
+    std::cout<<"dSpeed= "<<dSpeed<<std::endl;
+
+    // givlog->debug("SPEED","dSpeed is %f",dSpeed);
+    phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
+
+
+
+
+
+    gps_decition->wheel_angle = 0.0 - controlAng;
+
+
+
+    lastAngle=gps_decition->wheel_angle;
+
+    //   qDebug("decide1 time is %d",xTime.elapsed());
+
+    return gps_decition;
+}
+
+
+
+void iv::decition::DecideGps00::initMethods(){
+
+    pid_Controller= new PIDController();
+    ge3_Adapter = new Ge3Adapter();
+    qingyuan_Adapter = new QingYuanAdapter();
+    vv7_Adapter = new VV7Adapter();
+    zhongche_Adapter = new ZhongcheAdapter();
+    bus_Adapter = new BusAdapter();
+    hapo_Adapter = new HapoAdapter();
+
+
+    mpid_Controller.reset(pid_Controller);
+    mge3_Adapter.reset(ge3_Adapter);
+    mqingyuan_Adapter.reset(qingyuan_Adapter);
+    mvv7_Adapter.reset(vv7_Adapter);
+    mzhongche_Adapter.reset(zhongche_Adapter);
+    mbus_Adapter.reset(bus_Adapter);
+    mhapo_Adapter.reset(hapo_Adapter);
+
+    frenetPlanner = new FrenetPlanner();
+    //    laneChangePlanner = new LaneChangePlanner();
+
+    mfrenetPlanner.reset(frenetPlanner);
+    //    mlaneChangePlanner.reset(laneChangePlanner);
+
+}
+
+
+void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {
+
+    pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);
+
+    if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
+        ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
+        qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
+        vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
+        zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
+        bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
+        hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
+}
+
+
+
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
+    vector<iv::Point2D> trace;
+    //	int  PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+    /*if (PathPoint != -1)
+        lastGpsIndex = PathPoint;*/
+
+    if (gpsMapLine.size() > 600 && PathPoint >= 0) {
+        int aimIndex;
+        if(circleMode){
+            aimIndex=PathPoint+600;
+        }else{
+            aimIndex=min((PathPoint+600),(int)gpsMapLine.size());
+        }
+
+        for (int i = PathPoint; i < aimIndex; i++)
+        {
+            int index = i % gpsMapLine.size();
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
+            pt.x += offset_real * 0.032;
+            pt.v1 = (*gpsMapLine[index]).speed_mode;
+            pt.v2 = (*gpsMapLine[index]).mode2;
+            pt.roadMode=(*gpsMapLine[index]).roadMode;
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+
+
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            //csvv7
+            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
+            {
+                readyParkMode = true;
+                DecideGps00::finishPointNum = index;
+            }
+
+            //			if (pt.v1 == 22 || pt.v1 == 23)
+            //			{
+            //				readyParkMode = true;
+            //				DecideGps00::finishPointNum = i;
+            //			}
+
+
+            switch (pt.v1)
+            {
+            case 0:
+                pt.speed = 80;
+                break;
+            case 1:
+                pt.speed = 25;
+                break;
+            case 2:
+                pt.speed =25;
+                break;
+            case 3:
+                pt.speed = 20;
+                break;
+            case 4:
+                pt.speed =18;
+                break;
+            case 5:
+                pt.speed = 18;
+                break;
+            case 7:
+                pt.speed = 10;
+                break;
+            case 22:
+                pt.speed = 5;
+                break;
+            case 23:
+                pt.speed = 5;
+                break;
+            default:
+                break;
+            }
+            trace.push_back(pt);
+        }
+    }
+    return trace;
+}
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
+
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        double avoidLenth = abs(offset);
+        if (offset<0)
+        {
+            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+            ptLeft.speed = gpsTrace[j].speed;
+            ptLeft.v1 = gpsTrace[j].v1;
+            ptLeft.v2 = gpsTrace[j].v2;
+            trace.push_back(ptLeft);
+        }
+        else
+        {
+            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+            ptRight.speed = gpsTrace[j].speed;
+            ptRight.v1 = gpsTrace[j].v1;
+            ptRight.v2 = gpsTrace[j].v2;
+
+
+            trace.push_back(ptRight);
+        }
+    }
+    return trace;
+}
+
+
+double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
+    double angle=0;
+    if ( abs(iv::decition::DecideGps00().minDis) < 20 && abs(iv::decition::DecideGps00().maxAngle) < 100)
+    {
+        //   angle = iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
+        pid_Controller->getControlDecition(gpsIns, gpsTrace,  -1,  -1,  -1, true, false, &decition);
+        angle= decition->wheel_angle;
+    }
+    return angle;
+}
+
+double iv::decition::DecideGps00::getSpeed(std::vector<Point2D> gpsTrace) {
+    double speed=0;
+    int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
+    speed = gpsTrace[speedPoint].speed;
+    for (int i = 0; i < speedPoint; i++) {
+        speed = min(speed, gpsTrace[i].speed);
+    }
+    return speed;
+}
+
+
+
+
+//void iv::decition::DecideGps00::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
+//
+//	if (!obsRadars.empty())
+//	{
+//		esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
+//
+//		if (esrIndex != -1)
+//		{
+//			 esrDistance = obsRadars[esrIndex].nomal_y;
+//
+//
+//
+//			obsSpeed = obsRadars[esrIndex].speed_y;
+//
+//		}
+//		else {
+//			esrDistance = -1;
+//		}
+//
+//	}
+//	else
+//	{
+//		esrIndex = -1;
+//		esrDistance = -1;
+//	}
+//	if (esrDistance < 0) {
+//		ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
+//	}
+//	else {
+//		ODS("\n------------------>ESR障碍物距离:%f   ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
+//	}
+//
+//	ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
+//}
+
+
+
+void iv::decition::DecideGps00::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
+
+
+    int esrPathpoint;
+
+    esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint);
+
+    if (esrIndex != -1)
+    {
+        //优化
+        //        double distance = 0.0;
+        //        for(int i=0; i<esrPathpoint; ++i){
+        //            distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
+        //        }
+        //        esrDistance = distance - Esr_Y_Offset;
+        //        if(esrDistance<=0){
+        //            esrDistance=1;
+        //        }
+
+
+        esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
+        obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
+
+    }
+    else {
+        esrDistance = -1;
+    }
+}
+
+
+
+
+void iv::decition::DecideGps00::getEsrObsDistanceAvoid() {
+
+    esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);
+
+    if (esrIndexAvoid != -1)
+    {
+        esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
+        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+
+    }
+    else {
+        esrDistanceAvoid = -1;
+    }
+
+    if (esrDistanceAvoid < 0) {
+        std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
+    }
+    else {
+        std::cout << "\nESR障碍物距离Avoid:%f   %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
+    }
+
+    std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
+}
+
+
+
+
+double iv::decition::DecideGps00::limitAngle(double speed, double angle) {
+    double preAngle = angle;
+
+    if (speed > 15)
+    {
+        if (preAngle > 350)
+        {
+            preAngle = 350;
+        }
+        if (preAngle < -350)
+        {
+            preAngle = -350;
+        }
+    }
+    if (speed > 22)
+    {
+        if (preAngle > 200)
+        {
+            preAngle = 200;
+        }
+        if (preAngle < -200)
+        {
+            preAngle = -200;
+        }
+    }
+    if (speed > 25)
+    {
+        if (preAngle > 150)
+        {
+            preAngle = 150;
+        }
+        if (preAngle < -150)
+        {
+            preAngle = -150;
+        }
+    }
+    if (speed > 30)
+    {
+        if (preAngle > 70)
+        {
+            preAngle = 70;
+        }
+        if (preAngle < -70)
+        {
+            preAngle = -70;
+        }
+    }
+    if (speed > 45)                     //20
+    {
+        if (preAngle > 15)
+        {
+            preAngle = 15;
+        }
+        if (preAngle < -15)
+        {
+            preAngle = -15;
+        }
+    }
+    return preAngle;
+}
+
+
+
+double iv::decition::DecideGps00::limitSpeed(double angle, double speed) {
+
+    if (abs(angle) > 500 && speed > 8) speed = 8;
+    else if (abs(angle) > 350 && speed > 14) speed = 14;
+    else if (abs(angle) > 200 && speed > 21) speed = 21;
+    else if (abs(angle) > 150 && speed > 24) speed = 24;
+    else if (abs(angle) > 60 && speed > 29) speed = 29;
+    else if (abs(angle) > 20 && speed > 34) speed = 34;
+    return max(0.0, speed);
+}
+
+
+bool iv::decition::DecideGps00::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {
+
+    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+Veh_Lenth)||
+            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
+    {
+        return false;
+    }
+
+    if (roadNum-roadNow>1)
+    {
+        for (int i = roadNow+1; i < roadNum; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow-roadNum>1)
+    {
+        for (int i = roadNow-1; i >roadNum; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+
+
+bool iv::decition::DecideGps00::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
+    //lsn
+    if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
+    {
+        return false;
+    }
+    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
+            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
+    {
+        return false;
+    }
+
+
+    if (roadNum - roadNow>1)
+    {
+        for (int i = roadNow + 1; i < roadNum; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow - roadNum>1)
+    {
+        for (int i = roadNow - 1; i >roadNum; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+
+void iv::decition::DecideGps00::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
+
+
+
+    if (lidarGridPtr == NULL)
+    {
+        iv::decition::DecideGps00::lidarDistanceAvoid = iv::decition::DecideGps00::lastLidarDisAvoid;
+    }
+    else {
+        obsPointAvoid = Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr);
+        iv::decition::DecideGps00::lastLidarDisAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+    }
+    std::cout << "\nLidarAvoid距离:%f\n" << iv::decition::DecideGps00::lidarDistanceAvoid << std::endl;
+    getEsrObsDistanceAvoid();
+
+    //lidarDistanceAvoid = -1;   //20200103 apollo_fu
+
+    if (esrDistanceAvoid>0 && iv::decition::DecideGps00::lidarDistanceAvoid > 0)
+    {
+        if (iv::decition::DecideGps00::lidarDistanceAvoid >= esrDistanceAvoid)
+        {
+            obsDistanceAvoid = esrDistanceAvoid;
+            obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+        }
+        else if (!ServiceCarStatus.obs_radar.empty())
+        {
+            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+            obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
+            std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+        }
+        else
+        {
+            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+            obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
+            std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+        }
+    }
+    else if (esrDistanceAvoid>0)
+    {
+        obsDistanceAvoid = esrDistanceAvoid;
+        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+    }
+    else if (iv::decition::DecideGps00::lidarDistanceAvoid > 0)
+    {
+        obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+        obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
+        std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+    }
+    else {
+        obsDistanceAvoid = esrDistanceAvoid;
+        obsSpeedAvoid = 0 - secSpeed;
+    }
+
+    std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;
+
+
+
+
+
+    if (iv::decition::DecideGps00::obsDistanceAvoid <0 && obsLostTimeAvoid<4)
+    {
+        iv::decition::DecideGps00::obsDistanceAvoid = iv::decition::DecideGps00::lastDistanceAvoid;
+        obsLostTimeAvoid++;
+    }
+    else
+    {
+        obsLostTimeAvoid = 0;
+        iv::decition::DecideGps00::lastDistanceAvoid = -1;
+    }
+
+
+
+
+    if (obsDistanceAvoid>0)
+    {
+        iv::decition::DecideGps00::lastDistanceAvoid = obsDistanceAvoid;
+    }
+
+    std::cout << "\nODSAvoid距离:%f\n" << iv::decition::DecideGps00::obsDistanceAvoid << std::endl;
+}
+
+void iv::decition::DecideGps00::init() {
+    for (int i = 0; i < roadSum; i++)
+    {
+        lastEsrIdVector.push_back(-1);
+        lastEsrCountVector.push_back(0);
+        GPS_INS gps_ins;
+        gps_ins.gps_x = 0;
+        gps_ins.gps_y = 0;
+        startAvoidGpsInsVector.push_back(gps_ins);
+        avoidMinDistanceVector.push_back(0);
+    }
+}
+
+
+
+#include <QTime>
+
+void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+    //    QTime xTime;
+    //   xTime.start();
+    //    qDebug("time 0 is %d ",xTime.elapsed());
+    double obs,obsSd;
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+
+    obsSd= obsPoint.obs_speed_y;
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+    //   qDebug("time 3 is %d ",xTime.elapsed());
+
+}
+
+
+
+
+//1220
+void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                     const std::vector<GPSData> gpsMapLine) {
+
+    double obs,obsSd;
+
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        obsPoint = Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
+        }
+        if(abs(obsPoint.y)>lidarXiuZheng)
+            lidarDistance = abs(obsPoint.y)-lidarXiuZheng;   //激光距离推到车头 1220
+
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    if(lidarDistance==500){
+        lidarDistance=-1;
+    }
+
+
+
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+
+    obs=lidarDistance;
+    //	obsSpeed = 0 - secSpeed;
+    obsSd = 0 -secSpeed;
+
+
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+
+    if (obsDistance <0 && obsLostTime<4)
+    {
+        obsDistance = lastDistance;
+        obsLostTime++;
+    }
+    else
+    {
+        obsLostTime = 0;
+        lastDistance = -1;
+    }
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+}
+
+
+
+void iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
+    double preObsDis;
+
+    if(!lidar_per.empty()){
+        preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
+        lastPreObsDistance=preObsDis;
+
+    }else{
+        preObsDis=lastPreObsDistance;
+    }
+    if(preObsDis<obsDistance){
+        obsDistance=preObsDis;
+        lastDistance=obsDistance;
+    }
+}
+
+int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+    roadPre = -1;
+
+    if (roadNow<roadOri)
+    {
+        for (int i = 0; i < roadNow; i++)
+        {
+            gpsTraceAvoid.clear();
+            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+            avoidX = (roadWidth)*(roadOri - i);
+            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        }
+
+
+        for (int i = roadOri+1; i < roadSum; i++)
+        {
+            gpsTraceAvoid.clear();
+            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+            avoidX = (roadWidth)*(roadOri - i);
+            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+        }
+    }
+    else if (roadNow>roadOri)
+    {
+        for (int i = 0; i < roadOri; i++)
+        {
+            gpsTraceAvoid.clear();
+            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
+            avoidX = (roadWidth)*(roadOri - i);
+            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        }
+        for (int i = roadNow + 1; i < roadSum; i++)
+        {
+            gpsTraceAvoid.clear();
+            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+            avoidX = (roadWidth)*(roadOri - i);
+            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        }
+
+    }
+
+    else
+    {
+        for (int i = 0; i < roadOri; i++)
+        {
+            gpsTraceAvoid.clear();
+            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
+            avoidX = (roadWidth)*(roadOri - i);
+            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        }
+        for (int i = roadOri + 1; i < roadSum; i++)
+        {
+            gpsTraceAvoid.clear();
+            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
+            avoidX = (roadWidth)*(roadOri - i);
+            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
+            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        }
+
+    }
+
+    if (lidarGridPtr!=NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+
+    for(int i=0; i<roadSum;i++){
+        std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;
+
+    }
+
+    checkAvoidObsTimes++;
+    if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
+    {
+        return - 1;
+    }
+
+
+    for (int i = 1; i < roadSum; i++)
+    {
+        if (roadNow + i < roadSum) {
+            avoidX = (roadOri-roadNow-i)*roadWidth;
+            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
+            {
+                /*if (roadNow==roadOri)
+                {
+                    avoidRunDistance = obsDisVector[roadNow] + Veh_Lenth;
+                    startAvoid_gps_ins = now_gps_ins;
+                }	*/
+                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + Veh_Lenth;
+                startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                roadPre = roadNow + i;
+                return roadPre;
+            }
+        }
+
+        if (roadNow - i >= 0)
+        {
+            avoidX = (roadOri - roadNow+i)*roadWidth;
+            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
+            {
+                /*if (roadNow == roadOri)
+                {
+                    avoidRunDistance = obsDisVector[roadNow] + Veh_Lenth;
+                    startAvoid_gps_ins = now_gps_ins;
+                }*/
+                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + Veh_Lenth;
+                startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                roadPre = roadNow - i;
+                return roadPre;
+            }
+        }
+
+    }
+    return roadPre;
+}
+
+int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+    roadPre = -1;
+
+    computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+
+
+
+    if (roadNow>roadOri+1)
+    {
+        for (int i = roadOri+1; i < roadNow; i++)
+        {
+            gpsTraceBack.clear();
+            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
+            avoidX = (roadWidth)*(roadOri - i);
+            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        }
+    }
+    else if (roadNow < roadOri - 1) {
+
+        for (int i = roadOri - 1; i > roadNow; i--)
+        {
+            gpsTraceBack.clear();
+            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
+            avoidX = (roadWidth)*(roadOri - i);
+            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        }
+
+    }
+
+
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedBackLidar = true;
+    }
+
+
+
+    checkBackObsTimes++;
+    if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
+    {
+        return -1;
+    }
+
+
+
+
+
+
+
+    //if ((GetDistance(now_gps_ins, startAvoid_gps_ins)>avoidRunDistance)&&
+    //(checkReturnEnable(avoidX, lidarGridPtr,roadOri)))
+    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)) &&
+            (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
+    {
+        roadPre = roadOri;
+        return roadPre;
+    }
+
+    if (roadNow-roadOri>1)
+    {
+        for (int i = roadOri + 1;i < roadNow;i++) {
+            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  avoidMinDistanceVector[i])&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
+            {
+                roadPre = i;
+                return roadPre;
+            }
+        }
+    }
+
+    else if (roadNow <roadOri-1)
+    {
+        for (int i = roadOri - 1;i > roadNow;i--) {
+            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  30.0))
+            {
+                roadPre = i;
+                return roadPre;
+            }
+        }
+    }
+
+    return roadPre;
+}
+
+
+double iv::decition::DecideGps00::trumpet() {
+    if (trumpetFirstCount)
+    {
+        trumpetFirstCount = false;
+        trumpetLastTime= GetTickCount();
+        trumpetTimeSpan = 0.0;
+    }
+    else
+    {
+        trumpetStartTime= GetTickCount();
+        trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
+        trumpetLastTime = trumpetStartTime;
+    }
+
+    return trumpetTimeSpan;
+}
+
+
+
+
+double iv::decition::DecideGps00::transferP() {
+    if (transferFirstCount)
+    {
+        transferFirstCount = false;
+        transferLastTime= GetTickCount();
+        transferTimeSpan = 0.0;
+    }
+    else
+    {
+        transferStartTime= GetTickCount();
+        transferTimeSpan += transferStartTime - transferLastTime;
+        transferLastTime = transferStartTime;
+    }
+
+    return transferTimeSpan;
+}
+
+
+
+void  iv::decition::DecideGps00::handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins) {
+
+    if (abs(now_gps_ins.speed)>0.1)
+    {
+        decition->accelerator = 0;
+        decition->brake = 20;
+        decition->wheel_angle = 0;
+
+    }
+    else
+    {
+
+        decition->accelerator = 0;
+        decition->brake = 20;
+        decition->wheel_angle = 0;
+        handPark = true;
+        handParkTime = duringTime;
+    }
+
+}
+
+
+
+
+void iv::decition::DecideGps00::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins) {
+    gmapsL.clear();
+    gmapsR.clear();
+    for (int i = 0; i < 31; i++)
+    {
+        std::vector<iv::GPSData> gpsMapLineBeside;
+        //	gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
+        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);
+
+        gmapsL.push_back(gpsMapLineBeside);
+    }
+
+
+    for (int i = 0; i < 31; i++)
+    {
+        std::vector<iv::GPSData> gpsMapLineBeside;
+        //		gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
+        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);
+
+        gmapsR.push_back(gpsMapLineBeside);
+    }
+
+}
+
+
+bool iv::decition::DecideGps00::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {
+
+    if (lidarGridPtr == NULL)
+    {
+        return false;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+
+
+
+
+        obsPoint = Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
+        double lidarDistance = obsPoint.y - 2.5;   //激光距离推到车头
+        //     ODS("\n超车雷达距离:%f\n", lidarDistance);
+        if (lidarDistance >-20 && lidarDistance<35)
+        {
+            checkChaoCheBackCounts = 0;
+            return false;
+        }
+        else {
+            checkChaoCheBackCounts++;
+        }
+
+        if (checkChaoCheBackCounts>2) {
+            checkChaoCheBackCounts = 0;
+            return true;
+        }
+    }
+
+    return false;
+
+}
+
+void iv::decition::DecideGps00::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s){
+    Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);
+
+    ServiceCarStatus.group_x_local=pt.x;
+    //  ServiceCarStatus.group_y_local=pt.y;
+    ServiceCarStatus.group_y_local=s;
+    if(realspeed<0.36){
+        ServiceCarStatus.group_velx_local=0;
+        ServiceCarStatus.group_vely_local=0;
+    }else{
+        ServiceCarStatus.group_velx_local=realspeed*sin(theta)/3.6;
+        ServiceCarStatus.group_vely_local=realspeed*cos(theta)/3.6;
+    }
+
+
+    ServiceCarStatus.group_pathpoint=PathPoint;
+
+}
+
+
+
+float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+                                                           int pathpoint,float secSpeed,float dSpeed){
+    float traffic_speed=200;
+    float traffic_dis=0;
+    float passTime;
+    float passSpeed;
+    bool passEnable=false;
+
+    if(abs(secSpeed)<0.1){
+        secSpeed=0;
+    }
+
+
+
+    if(pathpoint <= traffic_light_pathpoint){
+        for(int i=pathpoint;i<traffic_light_pathpoint;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }else{
+        for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+        for(int i=0;i<traffic_light_pathpoint;i++){
+            traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    //    if(traffic_light_color != 0)
+    //    {
+    //        int a = 3;
+    //    }
+
+
+    if(traffic_light_color==0 && traffic_dis<10){
+        traffic_speed=0;
+    }
+    //    else   //20200108
+    //    {
+    //        traffic_speed=10;
+    //    }
+    return  traffic_speed;
+
+    passSpeed = min((float)(dSpeed/3.6),secSpeed);
+    passTime = traffic_dis/(dSpeed/3.6);
+
+
+
+
+
+    switch(traffic_light_color){
+    case 0:
+        if(passTime>traffic_light_time+1 && traffic_dis>10){
+            passEnable=true;
+        }else{
+            passEnable=false;
+        }
+        break;
+    case 1:
+        if(passTime<traffic_light_time-1 && traffic_dis<10){
+            passEnable=true;
+        }else{
+            passEnable = false;
+        }
+        break;
+    case 2:
+        if(passTime<traffic_light_time){
+            passEnable= true;
+        }else{
+            passEnable=false;
+        }
+        break;
+
+
+    default:
+        break;
+    }
+
+    if(!passEnable){
+        if(traffic_dis<5){
+            traffic_speed=0;
+        }else if(traffic_dis<10){
+            traffic_speed=5;
+        }else if(traffic_dis<20){
+            traffic_speed=15;
+        }else if(traffic_dis<30){
+            traffic_speed=25;
+        }else if(traffic_dis<50){
+            traffic_speed=30;
+        }
+    }
+    return traffic_speed;
+
+}
+
+void iv::decition::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
+{
+    //    Point2D obsCombinePoint = Point2D(-1,-1);
+    iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
+    double obsSd;
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        //        lidarDistance = obsPoint.y-2.5;   //激光距离推到车头
+        iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
+        lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;
+
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    FrenetPoint esr_obs_frenet_point;
+    getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+    if(esrDistance<0){
+        esrDistance=500;
+    }
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
+    myesrDistance = esrDistance;
+
+    if(lidarDistance==500){
+        lidarDistance=-1;
+    }
+    if(esrDistance==500){
+        esrDistance=-1;
+    }
+
+    ServiceCarStatus.mRadarObs = esrDistance;
+    ServiceCarStatus.mLidarObs = lidarDistance;
+
+    //    //zhuanwan pingbi haomibo
+    //    if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
+    //        esrDistance=-1;
+    //    }
+
+    if (esrDistance>0 && lidarDistance > 0)
+    {
+        if (lidarDistance >= esrDistance)
+        {
+            obs = esrDistance;
+            //            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
+            obsSd = obsSpeed;
+            //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
+            //            obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+        }
+        else if (!ServiceCarStatus.obs_radar.empty())
+        {
+            obs = lidarDistance;
+            //            obsCombinePoint = obsPoint;
+            //            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
+            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
+            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
+        }
+        else
+        {
+            obs=lidarDistance;
+            //            obsCombinePoint = obsPoint;
+            obsSd = 0 -secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
+            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
+        }
+    }
+    else if (esrDistance>0)
+    {
+        obs = esrDistance;
+        //        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
+        obsSd = obsSpeed;
+        //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
+        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+    }
+    else if (lidarDistance > 0)
+    {
+        obs = lidarDistance;
+        //        obsCombinePoint = obsPoint;
+        obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
+        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
+    }
+    else {
+        obs = esrDistance;
+        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+        obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
+    }
+
+    obsDistance=obs;
+    obsSpeed=obsSd;
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    if(obs<0){
+        obsDistance=500;
+    }else{
+        obsDistance=obs;
+    }
+}
+
+void iv::decition::DecideGps00::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
+
+
+    esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum);
+
+    if (esrIndex != -1)
+    {
+        esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
+        obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
+
+    }
+    else {
+        esrDistance = -1;
+    }
+}
+
+void iv::decition::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point, FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {
+
+    esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps);
+
+    if (esrIndex != -1)
+    {
+        //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
+        //严格来说应是 esrDistance=障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
+        esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset;  //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。
+
+        double speedx=ServiceCarStatus.obs_radar[esrIndex].speed_x;  //障碍物相对于车辆x轴的速度
+        double speedy=ServiceCarStatus.obs_radar[esrIndex].speed_y;  //障碍物相对于车辆y轴的速度
+        double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
+        //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
+        //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
+        double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);
+
+        obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
+    }
+    else {
+        esrDistance = -1;
+    }
+}
+
+void iv::decition::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine){
+    v2xTrafficVector.clear();
+    for (int var = 0; var < gpsMapLine.size(); var++) {
+        if(gpsMapLine[var]->roadMode==6){
+            v2xTrafficVector.push_back(var);
+        }
+    }
+}
+
+float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
+                                                             int pathpoint,float secSpeed,float dSpeed, bool circleMode){
+    float trafficSpeed=200;
+    int nearTraffixPoint=-1;
+    float nearTrafficDis=0;
+    int traffic_color=0;
+    int traffic_time=0;
+    bool passThrough=false;
+    float dSecSpeed=dSpeed/3.6;
+
+    if(v2xTrafficVector.empty()){
+        return trafficSpeed;
+    }
+    if(!circleMode){
+        if(pathpoint>v2xTrafficVector.back()){
+            return trafficSpeed;
+        }else {
+            for(int i=0; i< v2xTrafficVector.size();i++){
+                if (pathpoint<= v2xTrafficVector[i]){
+                    nearTraffixPoint=v2xTrafficVector[i];
+                    break;
+                }
+            }
+        }
+    }else if(circleMode){
+        if(pathpoint>v2xTrafficVector.back()){
+            nearTraffixPoint=v2xTrafficVector[0];
+        }else {
+            for(int i=0; i< v2xTrafficVector.size();i++){
+                if (pathpoint<= v2xTrafficVector[i]){
+                    nearTraffixPoint=v2xTrafficVector[i];
+                    break;
+                }
+            }
+        }
+    }
+
+
+    if(nearTraffixPoint!=-1){
+        for(int i=pathpoint;i<nearTraffixPoint;i++){
+            nearTrafficDis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    if(nearTrafficDis>50){
+        return trafficSpeed;
+    }
+
+
+    int roadMode = gpsMapLine[pathpoint]->roadMode;
+    if(roadMode==14 || roadMode==16){
+        traffic_color=trafficLight.leftColor;
+        traffic_time=trafficLight.leftTime;
+    }else if(roadMode==15 ||roadMode==17){
+        traffic_color=trafficLight.rightColor;
+        traffic_time=trafficLight.rightTime;
+    }else {
+        traffic_color=trafficLight.straightColor;
+        traffic_time=trafficLight.straightTime;
+    }
+
+
+    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
+    if(passThrough){
+        return trafficSpeed;
+    }else{
+        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
+        if(nearTrafficDis<6){
+            float decelerate =0-( secSpeed*secSpeed*0.5/nearTrafficDis);
+            minDecelerate=min(minDecelerate,decelerate);
+        }
+        return trafficSpeed;
+    }
+
+    return trafficSpeed;
+}
+
+bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed){
+    float passTime=0;
+    if (trafficColor==2 || trafficColor==3){
+        return false;
+    }else if(trafficColor==0){
+        return true;
+    }else{
+        passTime=trafficDis/dSecSpeed;
+        if(passTime+1< trafficTime){
+            return true;
+        }else{
+            return false;
+        }
+
+    }
+
+}
+
+float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
+    float limit=200;
+    if(trafficDis<10){
+        limit = 0;
+    }else if(trafficDis<15){
+        limit = 5;
+    }else if(trafficDis<20){
+        limit=10;
+    }else if(trafficDis<30){
+        limit=15;
+    }
+    return limit;
+}
+
+void iv::decition::DecideGps00::adjuseGpsLidarPosition(){
+
+    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+
+}

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -1,248 +1,248 @@
-#pragma once
-#ifndef _IV_DECITION__DECIDE_GPS_00_
-#define _IV_DECITION__DECIDE_GPS_00_
-
-#include <common_sf/gps_type.h>
-#include <decition/decition_type.h>
-#include <common_sf/obstacle_type.h>
-#include <vector>
-#include <decition/adc_tools/gnss_coordinate_convert.h>
-#include <decition/adc_planner/frenet_planner.h>
-#include<decition/adc_controller/pid_controller.h>
-#include <decition/adc_adapter/ge3_adapter.h>
-#include <decition/adc_adapter/qingyuan_adapter.h>
-#include <decition/adc_adapter/vv7_adapter.h>
-#include <decition/adc_adapter/zhongche_adapter.h>
-#include <decition/adc_adapter/bus_adapter.h>
-#include <decition/adc_adapter/hapo_adapter.h>
-#include "perception/perceptionoutput.h"
-#include "ivlog.h"
-#include <memory>
-
-namespace iv {
-namespace decition {
-//根据传感器传输来的信息作出决策
-class DecideGps00 {
-public:
-    DecideGps00();
-    ~DecideGps00();
-
-    static double minDis;
-    static double maxAngle;
-    static int lastGpsIndex;
-    static double lidarDistance;
-    static double myesrDistance;
-    static double lastLidarDis;
-    static double lastLidarDisAvoid;
-
-    static double lastPreObsDistance;
-
-    static int gpsLineParkIndex;
-    static int gpsMapParkIndex;
-
-    static double lastDistance;
-
-    static float xiuzhengCs;
-
-    static double controlAng;
-    double laneAng;
-    static double controlSpeed;
-    static double controlBrake;
-
-    static bool parkMode;
-    static bool readyParkMode;
-
-    static bool zhucheMode;
-    static bool readyZhucheMode;
-    static bool hasZhuched;
-    static double Iv;
-    static double lastEv;
-    static double lastVt;
-    static double lastU;
-    static double obsDistance;
-    static double obsSpeed;
-    static double lidarDistanceAvoid;
-    static double obsDistanceAvoid;
-    static double lastDistanceAvoid;
-
-    GPS_INS group_ori_gps;
-    GPS_INS startAvoid_gps_ins;
-    static int finishPointNum;
-    static int zhuchePointNum;
-    double avoidRunDistance=0;
-
-    std::vector<iv::GPS_INS> startAvoidGpsInsVector;
-    std::vector<double> avoidMinDistanceVector;
-    std::vector<int> v2xTrafficVector;
-
-    ///kkcai, 2020-01-03
-    //bool isFirstRun = true;
-    static bool isFirstRun;
-    /////////////////////////////////////
-
-//    static bool startingFlag;//起步标志，在起步时需要进行frenet规划。
-    static float minDecelerate;
-
-
-    double startTime = 0;
-    double firstTime = 0;
-
-    bool circleMode=false;
-
-    int avoidTimes = 0;
-    int backTimes = 0;
-
-    int checkAvoidObsTimes = 0;
-    int checkBackObsTimes = 0;
-
-    bool hasCheckedAvoidLidar=false;
-    bool hasCheckedBackLidar=false;
-
-    bool personWaited = false;
-    bool roadLightWaited = false;
-
-    double decision_Angle = 0;
-    double lastAngle=0;
-
-    iv::Point2D obsPoint;
-
-    int checkAvoidTimes=0;
-    int checkBackTimes=0;
-    int chaocheCounts=0;
-
-    static int PathPoint;
-    float lastGroupOffsetX=0.0;
-
-    GPS_INS traffic_light_gps;
-    int traffic_light_pathpoint;
-
-    bool changingDangWei=false;
-
-    BaseController *pid_Controller;
-    BaseAdapter *ge3_Adapter;
-    BaseAdapter *qingyuan_Adapter;
-    BaseAdapter *vv7_Adapter;
-    BaseAdapter *zhongche_Adapter;
-    BaseAdapter *bus_Adapter;
-    BaseAdapter *hapo_Adapter;
-
-    std::shared_ptr<BaseController> mpid_Controller;
-    std::shared_ptr<BaseAdapter> mge3_Adapter;
-    std::shared_ptr<BaseAdapter> mqingyuan_Adapter;
-    std::shared_ptr<BaseAdapter> mvv7_Adapter;
-    std::shared_ptr<BaseAdapter> mzhongche_Adapter;
-    std::shared_ptr<BaseAdapter> mbus_Adapter;
-    std::shared_ptr<BaseAdapter> mhapo_Adapter;
-
-	FrenetPlanner *frenetPlanner;
-//    BasePlanner *laneChangePlanner;
-	
-	std::shared_ptr<FrenetPlanner> mfrenetPlanner;
-//    std::shared_ptr<BasePlanner> mlaneChangePlanner;
-
-
-    Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,
-                              std::vector<iv::Perception::PerceptionOutput> lidar_per,iv::TrafficLight trafficLight);
-
-    void initMethods();
-    static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);
-
-
-    static double getAngle(std::vector<Point2D> gpsTrace);
-    double getAngle(std::vector<Point2D> gpsTrace, GPS_INS gpsIns,iv::decition::Decition decition);
-    static double getSpeed(std::vector<Point2D> gpsTrace);
-
-    static void getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
-    static void getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
-    static void getEsrObsDistanceAvoid();
-
-    void getObsAvoid(iv::LidarGridPtr lidarGridPtr);
-    void phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns );
-
-    double limitAngle(double speed, double angle);
-    double limitSpeed(double angle, double speed);
-
-    bool checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
-    bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
-
-    void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
-    static void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decition::FrenetPoint car_now_frenet_point,iv::decition::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
-
-    void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);
-    void computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr,const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
-
-    void predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed);
-
-    int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins,std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
-    int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
-
-    void getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins);
-
-    void  handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins);
-
-    bool checkChaoCheBack(iv::LidarGridPtr);
-    double trumpet();
-    double transferP();
-
-    bool nearStation;
-
-    void updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s);
-    void init();
-
-    std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);
-
-    std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);
-
-    float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
-                                   int pathpoint,float secSpeed,float dSpeed);
-    void getV2XTrafficPositionVector( const std::vector<GPSData> gpsMapLine);
-    float ComputeV2XTrafficLightSpeed(iv::TrafficLight TrafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
-                                   int pathpoint,float secSpeed,float dSpeed,bool circleMode);
-    float getTrafficLightSpeed(int traffic_light_color, int traffic_light_time, float dis);
-
-    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
-    float computeTrafficSpeedLimt(float trafficDis);
-    void adjuseGpsLidarPosition();
-
-    GPS_INS aim_gps_ins;
-    GPS_INS chaoche_start_gps;
-    bool is_arrivaled=false;
-
-    double chaocheObsDis = 0;
-    double preChaocheDis = 0;
-    double aimObsSpeed = 0;
-    double followSpeed = 30;
-    double chaocheSpeed = 50;
-    int checkChaoCheBackCounts = 0;
-
-    float lastTorque=0;
-    float splimit=-1;
-
-    float  ObsTimeSpan=-1;
-    double ObsTimeStart=-1;
-    double ObsTimeEnd=-1;
-    float ObsTimeWidth=1500;
-    std::vector<iv::Point2D> planTrace;
-private:
-    //  void changeRoad(int roadNum);
-
-
-};
-
-}
-}
-
-extern bool handPark;
-extern long handParkTime;
-extern bool rapidStop;
-extern int gpsMissCount;
-extern bool changeRoad;
-extern double avoidX;
-extern bool parkBesideRoad;
-extern double steerSpeed;
-extern bool transferPieriod;
-extern bool transferPieriod2;
-extern double traceDevi;
-#endif // !  _IV_DECITION__DECIDE_GPS_00_
-
-
+#pragma once
+#ifndef _IV_DECITION__DECIDE_GPS_00_
+#define _IV_DECITION__DECIDE_GPS_00_
+
+#include <common_sf/gps_type.h>
+#include <decition/decition_type.h>
+#include <common_sf/obstacle_type.h>
+#include <vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_planner/frenet_planner.h>
+#include<decition/adc_controller/pid_controller.h>
+#include <decition/adc_adapter/ge3_adapter.h>
+#include <decition/adc_adapter/qingyuan_adapter.h>
+#include <decition/adc_adapter/vv7_adapter.h>
+#include <decition/adc_adapter/zhongche_adapter.h>
+#include <decition/adc_adapter/bus_adapter.h>
+#include <decition/adc_adapter/hapo_adapter.h>
+#include "perception/perceptionoutput.h"
+#include "ivlog.h"
+#include <memory>
+
+namespace iv {
+namespace decition {
+//根据传感器传输来的信息作出决策
+class DecideGps00 {
+public:
+    DecideGps00();
+    ~DecideGps00();
+
+    static double minDis;
+    static double maxAngle;
+    static int lastGpsIndex;
+    static double lidarDistance;
+    static double myesrDistance;
+    static double lastLidarDis;
+    static double lastLidarDisAvoid;
+
+    static double lastPreObsDistance;
+
+    static int gpsLineParkIndex;
+    static int gpsMapParkIndex;
+
+    static double lastDistance;
+
+    static float xiuzhengCs;
+
+    static double controlAng;
+    double laneAng;
+    static double controlSpeed;
+    static double controlBrake;
+
+    static bool parkMode;
+    static bool readyParkMode;
+
+    static bool zhucheMode;
+    static bool readyZhucheMode;
+    static bool hasZhuched;
+    static double Iv;
+    static double lastEv;
+    static double lastVt;
+    static double lastU;
+    static double obsDistance;
+    static double obsSpeed;
+    static double lidarDistanceAvoid;
+    static double obsDistanceAvoid;
+    static double lastDistanceAvoid;
+
+    GPS_INS group_ori_gps;
+    GPS_INS startAvoid_gps_ins;
+    static int finishPointNum;
+    static int zhuchePointNum;
+    double avoidRunDistance=0;
+
+    std::vector<iv::GPS_INS> startAvoidGpsInsVector;
+    std::vector<double> avoidMinDistanceVector;
+    std::vector<int> v2xTrafficVector;
+
+    ///kkcai, 2020-01-03
+    //bool isFirstRun = true;
+    static bool isFirstRun;
+    /////////////////////////////////////
+
+//    static bool startingFlag;//起步标志，在起步时需要进行frenet规划。
+    static float minDecelerate;
+
+
+    double startTime = 0;
+    double firstTime = 0;
+
+    bool circleMode=false;
+
+    int avoidTimes = 0;
+    int backTimes = 0;
+
+    int checkAvoidObsTimes = 0;
+    int checkBackObsTimes = 0;
+
+    bool hasCheckedAvoidLidar=false;
+    bool hasCheckedBackLidar=false;
+
+    bool personWaited = false;
+    bool roadLightWaited = false;
+
+    double decision_Angle = 0;
+    double lastAngle=0;
+
+    iv::Point2D obsPoint;
+
+    int checkAvoidTimes=0;
+    int checkBackTimes=0;
+    int chaocheCounts=0;
+
+    static int PathPoint;
+    float lastGroupOffsetX=0.0;
+
+    GPS_INS traffic_light_gps;
+    int traffic_light_pathpoint;
+
+    bool changingDangWei=false;
+
+    BaseController *pid_Controller;
+    BaseAdapter *ge3_Adapter;
+    BaseAdapter *qingyuan_Adapter;
+    BaseAdapter *vv7_Adapter;
+    BaseAdapter *zhongche_Adapter;
+    BaseAdapter *bus_Adapter;
+    BaseAdapter *hapo_Adapter;
+
+    std::shared_ptr<BaseController> mpid_Controller;
+    std::shared_ptr<BaseAdapter> mge3_Adapter;
+    std::shared_ptr<BaseAdapter> mqingyuan_Adapter;
+    std::shared_ptr<BaseAdapter> mvv7_Adapter;
+    std::shared_ptr<BaseAdapter> mzhongche_Adapter;
+    std::shared_ptr<BaseAdapter> mbus_Adapter;
+    std::shared_ptr<BaseAdapter> mhapo_Adapter;
+
+	FrenetPlanner *frenetPlanner;
+//    BasePlanner *laneChangePlanner;
+	
+	std::shared_ptr<FrenetPlanner> mfrenetPlanner;
+//    std::shared_ptr<BasePlanner> mlaneChangePlanner;
+
+
+    Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,
+                              std::vector<iv::Perception::PerceptionOutput> lidar_per,iv::TrafficLight trafficLight);
+
+    void initMethods();
+    static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);
+
+
+    static double getAngle(std::vector<Point2D> gpsTrace);
+    double getAngle(std::vector<Point2D> gpsTrace, GPS_INS gpsIns,iv::decition::Decition decition);
+    static double getSpeed(std::vector<Point2D> gpsTrace);
+
+    static void getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
+    static void getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
+    static void getEsrObsDistanceAvoid();
+
+    void getObsAvoid(iv::LidarGridPtr lidarGridPtr);
+    void phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns );
+
+    double limitAngle(double speed, double angle);
+    double limitSpeed(double angle, double speed);
+
+    bool checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
+    bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
+
+    void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    static void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decition::FrenetPoint car_now_frenet_point,iv::decition::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+
+    void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);
+    void computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr,const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+
+    void predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed);
+
+    int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins,std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
+
+    void getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins);
+
+    void  handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins);
+
+    bool checkChaoCheBack(iv::LidarGridPtr);
+    double trumpet();
+    double transferP();
+
+    bool nearStation;
+
+    void updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s);
+    void init();
+
+    std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);
+
+    std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);
+
+    float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+                                   int pathpoint,float secSpeed,float dSpeed);
+    void getV2XTrafficPositionVector( const std::vector<GPSData> gpsMapLine);
+    float ComputeV2XTrafficLightSpeed(iv::TrafficLight TrafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
+                                   int pathpoint,float secSpeed,float dSpeed,bool circleMode);
+    float getTrafficLightSpeed(int traffic_light_color, int traffic_light_time, float dis);
+
+    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
+    float computeTrafficSpeedLimt(float trafficDis);
+    void adjuseGpsLidarPosition();
+
+    GPS_INS aim_gps_ins;
+    GPS_INS chaoche_start_gps;
+    bool is_arrivaled=false;
+
+    double chaocheObsDis = 0;
+    double preChaocheDis = 0;
+    double aimObsSpeed = 0;
+    double followSpeed = 30;
+    double chaocheSpeed = 50;
+    int checkChaoCheBackCounts = 0;
+
+    float lastTorque=0;
+    float splimit=-1;
+
+    float  ObsTimeSpan=-1;
+    double ObsTimeStart=-1;
+    double ObsTimeEnd=-1;
+    float ObsTimeWidth=1500;
+    std::vector<iv::Point2D> planTrace;
+private:
+    //  void changeRoad(int roadNum);
+
+
+};
+
+}
+}
+
+extern bool handPark;
+extern long handParkTime;
+extern bool rapidStop;
+extern int gpsMissCount;
+extern bool changeRoad;
+extern double avoidX;
+extern bool parkBesideRoad;
+extern double steerSpeed;
+extern bool transferPieriod;
+extern bool transferPieriod2;
+extern double traceDevi;
+#endif // !  _IV_DECITION__DECIDE_GPS_00_
+
+

src/decition/decition_brain_sf/decition/decide_line_00.h

@@ -1,252 +1,252 @@
-#pragma once
-#ifndef _IV_DECITION__DECIDE_LINE_00_
-#define _IV_DECITION__DECIDE_LINE_00_
-
-#include <common_sf/gps_type.h>
-#include <decition/decition_type.h>
-#include <common_sf/obstacle_type.h>
-#include<vector> 
-#include <decition/gnss_coordinate_convert.h>
-#include <decition/decide_gps_00.h>
-
-
-
-namespace iv {
-	namespace decition {
-		//根据传感器传输来的信息作出决策
-        class DecideLine00 {
-		public:
-            DecideLine00();
-            ~DecideLine00();
-
-			static  double minDis;
-			static double maxAngle;
-			static   int lastGpsIndex;
-			static double lidarDistance;
-            static double myesrDistance;
-			static double lastLidarDis;
-			static double lastLidarDisAvoid;
-
-			
-			
-
-			static  int gpsLineParkIndex;
-			static  int gpsMapParkIndex;
-
-
-			static double lastDistance;
-
-		
-
-			
-
-
-
-			static double controlAng;
-			static double controlSpeed;
-			static double controlBrake;
-
-			static bool parkMode;
-			static bool readyParkMode;
-
-			static bool zhucheMode;
-			static bool readyZhucheMode;
-			static bool hasZhuched;
-			static double Iv;
-			static double lastEv;
-			static double lastVt;
-			static double lastU;
-			static double obsDistance;
-			static double lidarDistanceAvoid;
-			static double obsDistanceAvoid;
-			static double lastDistanceAvoid;
-
-			GPS_INS startAvoid_gps_ins;
-			static int finishPointNum;
-			static int zhuchePointNum;
-			double avoidRunDistance=0;
-
-			std::vector<iv::GPS_INS> startAvoidGpsInsVector;
-			std::vector<double> avoidMinDistanceVector;
-
-
-			bool isFirstRun = true;
-			double startTime = 0;
-			double firstTime = 0;
-
-			int avoidTimes = 0;
-			int backTimes = 0;
-
-			int checkAvoidObsTimes = 0;
-			int checkBackObsTimes = 0;
-
-			bool hasCheckedAvoidLidar=false;
-			bool hasCheckedBackLidar=false;
-
-			bool personWaited = false;
-			bool roadLightWaited = false;
-
-			double decision_Angle = 0;
-            double lastAngle=0;
-
-			iv::Point2D obsPoint;
-
-			int checkAvoidTimes=0;
-			int checkBackTimes=0;
-
-			 static int PathPoint;
-//             float dSpeed=0;
-//             float dSecSpeed=0;
-
-			//		Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, std::vector<ObstacleBasic> obsRadars, iv::LidarGridPtr lidarGridPtr);
-
-            Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,std::vector<iv::Perception::PerceptionOutput> lidar_per);
-
-			static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex);
-             std::vector<Point2D> getGpsTraceByMobileEye(float a,float b, float c);
-		
-
-			static double getAngle(std::vector<Point2D> gpsTrace);
-			static double getSpeed(std::vector<Point2D> gpsTrace);
-
-			//		static void getEsrObs(std::vector<iv::ObstacleBasic> obsRadars);
-			static void getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
-			static void getEsrObsDistanceAvoid();
-
-			void getObsAvoid(iv::LidarGridPtr lidarGridPtr);
-
-            void phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDistance, double obsSpeed, GPS_INS gpsIns );
-
-			double limitAngle(double speed, double angle);
-			double limitSpeed(double angle, double speed);
-
-			bool checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
-			bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
-
-			void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum);
-
-			int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins);
-			int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine);
-			
-
-			void getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins);
-
-			void  handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins);
-
-
-			double trumpet();
-
-			void init();
-
-			std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);
-
-			GPS_INS aim_gps_ins;
-			bool is_arrivaled=false;
-
-
-
-
-
-            double offset_real = 0;
-            double realspeed;
-            double dSpeed;
-            double dSecSpeed;
-            double secSpeed;
-            double ac;
-
-
-
-
-
-
-            int esrLineIndex = -1;
-            int esrIndexAvoid = -1;
-            double obsSpeed = 0;
-            double obsSpeedAvoid = 0;
-
-            double esrLineDistance = -1;
-            double esrDistanceAvoid = -1;
-            int obsLostTime = 0;
-            int obsLostTimeAvoid = 0;
-
-
-
-
-            // double avoidTime = 0;
-
-
-            double avoidX =0;
-            float roadWidth = 4;
-            int roadSum =4;
-            int roadNow = 3;
-            int roadOri =3;
-            int roadPre = -1;
-            int lastRoadOri = 3;
-
-            int lightTimes = 0;
-
-
-            int lastRoadNum=1;
-
-            int stopCount = 0;
-
-            int gpsMissCount = 0;
-
-            bool rapidStop = false;
-
-            bool hasTrumpeted = false;
-
-
-            bool changingDangWei=false;
-
-            //实验手刹
-            bool handFirst = true;
-            double handTimeSpan = 0;
-            double handStartTime = 0;
-            double handLastTime = 0;
-            bool handPark = false;
-            long handParkTime=10000;
-
-            //喇叭
-            bool trumpetFirstCount=true;
-            double trumpetTimeSpan = 0;
-            double trumpetStartTime = 0;
-            double trumpetLastTime = 0;
-
-            bool busMode = false;
-
-            enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
-                waitAvoid ,shoushaTest,justRun} vehState;
-
-
-
-            std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri;
-
-
-            float  outGarageDistance=0;
-            double outGarageTimeLast=-1;
-            double outGarageTimeNow=-1;
-            float outGarageLong=10;
-
-            double waitGpsTimeStart=-1;
-            double waitGpsTimeNow=-1;
-            float waitGpsTimeSpan=-1;
-            float waitGpsTimeWidth=5000;
-            bool waitGps=false;
-
-
-		private:
-               iv::decition::DecideGps00* decitionGpstool ;			//决策器
-               std::shared_ptr<DecideGps00> mdecitionGpstool;
-		};
-
-	}
-}
-
-extern bool handPark;
-extern long handParkTime;
-extern bool rapidStop;
-extern int gpsMissCount;
-#endif // !  _IV_DECITION__DECIDE_LINE_00_
-
-
+#pragma once
+#ifndef _IV_DECITION__DECIDE_LINE_00_
+#define _IV_DECITION__DECIDE_LINE_00_
+
+#include <common_sf/gps_type.h>
+#include <decition/decition_type.h>
+#include <common_sf/obstacle_type.h>
+#include<vector> 
+#include <decition/gnss_coordinate_convert.h>
+#include <decition/decide_gps_00.h>
+
+
+
+namespace iv {
+	namespace decition {
+		//根据传感器传输来的信息作出决策
+        class DecideLine00 {
+		public:
+            DecideLine00();
+            ~DecideLine00();
+
+			static  double minDis;
+			static double maxAngle;
+			static   int lastGpsIndex;
+			static double lidarDistance;
+            static double myesrDistance;
+			static double lastLidarDis;
+			static double lastLidarDisAvoid;
+
+			
+			
+
+			static  int gpsLineParkIndex;
+			static  int gpsMapParkIndex;
+
+
+			static double lastDistance;
+
+		
+
+			
+
+
+
+			static double controlAng;
+			static double controlSpeed;
+			static double controlBrake;
+
+			static bool parkMode;
+			static bool readyParkMode;
+
+			static bool zhucheMode;
+			static bool readyZhucheMode;
+			static bool hasZhuched;
+			static double Iv;
+			static double lastEv;
+			static double lastVt;
+			static double lastU;
+			static double obsDistance;
+			static double lidarDistanceAvoid;
+			static double obsDistanceAvoid;
+			static double lastDistanceAvoid;
+
+			GPS_INS startAvoid_gps_ins;
+			static int finishPointNum;
+			static int zhuchePointNum;
+			double avoidRunDistance=0;
+
+			std::vector<iv::GPS_INS> startAvoidGpsInsVector;
+			std::vector<double> avoidMinDistanceVector;
+
+
+			bool isFirstRun = true;
+			double startTime = 0;
+			double firstTime = 0;
+
+			int avoidTimes = 0;
+			int backTimes = 0;
+
+			int checkAvoidObsTimes = 0;
+			int checkBackObsTimes = 0;
+
+			bool hasCheckedAvoidLidar=false;
+			bool hasCheckedBackLidar=false;
+
+			bool personWaited = false;
+			bool roadLightWaited = false;
+
+			double decision_Angle = 0;
+            double lastAngle=0;
+
+			iv::Point2D obsPoint;
+
+			int checkAvoidTimes=0;
+			int checkBackTimes=0;
+
+			 static int PathPoint;
+//             float dSpeed=0;
+//             float dSecSpeed=0;
+
+			//		Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, std::vector<ObstacleBasic> obsRadars, iv::LidarGridPtr lidarGridPtr);
+
+            Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,std::vector<iv::Perception::PerceptionOutput> lidar_per);
+
+			static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex);
+             std::vector<Point2D> getGpsTraceByMobileEye(float a,float b, float c);
+		
+
+			static double getAngle(std::vector<Point2D> gpsTrace);
+			static double getSpeed(std::vector<Point2D> gpsTrace);
+
+			//		static void getEsrObs(std::vector<iv::ObstacleBasic> obsRadars);
+			static void getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
+			static void getEsrObsDistanceAvoid();
+
+			void getObsAvoid(iv::LidarGridPtr lidarGridPtr);
+
+            void phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDistance, double obsSpeed, GPS_INS gpsIns );
+
+			double limitAngle(double speed, double angle);
+			double limitSpeed(double angle, double speed);
+
+			bool checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
+			bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
+
+			void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum);
+
+			int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins);
+			int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine);
+			
+
+			void getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins);
+
+			void  handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins);
+
+
+			double trumpet();
+
+			void init();
+
+			std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);
+
+			GPS_INS aim_gps_ins;
+			bool is_arrivaled=false;
+
+
+
+
+
+            double offset_real = 0;
+            double realspeed;
+            double dSpeed;
+            double dSecSpeed;
+            double secSpeed;
+            double ac;
+
+
+
+
+
+
+            int esrLineIndex = -1;
+            int esrIndexAvoid = -1;
+            double obsSpeed = 0;
+            double obsSpeedAvoid = 0;
+
+            double esrLineDistance = -1;
+            double esrDistanceAvoid = -1;
+            int obsLostTime = 0;
+            int obsLostTimeAvoid = 0;
+
+
+
+
+            // double avoidTime = 0;
+
+
+            double avoidX =0;
+            float roadWidth = 4;
+            int roadSum =4;
+            int roadNow = 3;
+            int roadOri =3;
+            int roadPre = -1;
+            int lastRoadOri = 3;
+
+            int lightTimes = 0;
+
+
+            int lastRoadNum=1;
+
+            int stopCount = 0;
+
+            int gpsMissCount = 0;
+
+            bool rapidStop = false;
+
+            bool hasTrumpeted = false;
+
+
+            bool changingDangWei=false;
+
+            //实验手刹
+            bool handFirst = true;
+            double handTimeSpan = 0;
+            double handStartTime = 0;
+            double handLastTime = 0;
+            bool handPark = false;
+            long handParkTime=10000;
+
+            //喇叭
+            bool trumpetFirstCount=true;
+            double trumpetTimeSpan = 0;
+            double trumpetStartTime = 0;
+            double trumpetLastTime = 0;
+
+            bool busMode = false;
+
+            enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
+                waitAvoid ,shoushaTest,justRun} vehState;
+
+
+
+            std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri;
+
+
+            float  outGarageDistance=0;
+            double outGarageTimeLast=-1;
+            double outGarageTimeNow=-1;
+            float outGarageLong=10;
+
+            double waitGpsTimeStart=-1;
+            double waitGpsTimeNow=-1;
+            float waitGpsTimeSpan=-1;
+            float waitGpsTimeWidth=5000;
+            bool waitGps=false;
+
+
+		private:
+               iv::decition::DecideGps00* decitionGpstool ;			//决策器
+               std::shared_ptr<DecideGps00> mdecitionGpstool;
+		};
+
+	}
+}
+
+extern bool handPark;
+extern long handParkTime;
+extern bool rapidStop;
+extern int gpsMissCount;
+#endif // !  _IV_DECITION__DECIDE_LINE_00_
+
+

src/decition/decition_brain_sf/decition/decide_line_00_.cpp

@@ -1,979 +1,979 @@
-#include <decition/decide_line_00.h>
-#include <decition/compute_00.h>
-#include <decition/gps_distance.h>
-#include <decition/decition_type.h>
-#include <decition/transfer.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-#include <control/can.h>
-#include <time.h>
-using namespace std;
-
-#define PI (3.1415926535897932384626433832795)
-
-iv::decition::DecideLine00::DecideLine00() {
-      decitionGpstool = new iv::decition::DecideGps00();
-      mdecitionGpstool.reset(decitionGpstool);
-}
-iv::decition::DecideLine00::~DecideLine00() {
-
-}
-
-
-int iv::decition::DecideLine00::PathPoint = -1;
-double iv::decition::DecideLine00::minDis = iv::MaxValue;
-double iv::decition::DecideLine00::maxAngle = iv::MinValue;
-//int iv::decition::DecideLine00::lastGpsIndex = iv::MaxValue;
-int iv::decition::DecideLine00::lastGpsIndex = 0;
-double iv::decition::DecideLine00::controlSpeed = 0;
-double iv::decition::DecideLine00::controlBrake = 0;
-double iv::decition::DecideLine00::controlAng = 0;
-double iv::decition::DecideLine00::Iv = 0;
-double iv::decition::DecideLine00::lastU = 0;
-double iv::decition::DecideLine00::lastVt = 0;
-double iv::decition::DecideLine00::lastEv = 0;
-
-int iv::decition::DecideLine00::gpsLineParkIndex = 0;
-int iv::decition::DecideLine00::gpsMapParkIndex = 0;
-double iv::decition::DecideLine00::lastDistance = MaxValue;
-double iv::decition::DecideLine00::obsDistance = -1;
-double iv::decition::DecideLine00::obsDistanceAvoid = -1;
-double iv::decition::DecideLine00::lastDistanceAvoid = -1;
-
-double iv::decition::DecideLine00::lidarDistance = -1;
-double iv::decition::DecideLine00::myesrDistance = -1;
-double iv::decition::DecideLine00::lastLidarDis = -1;
-
-bool iv::decition::DecideLine00::parkMode = false;
-bool iv::decition::DecideLine00::readyParkMode = false;
-
-bool iv::decition::DecideLine00::zhucheMode = false;
-bool iv::decition::DecideLine00::readyZhucheMode = false;
-bool iv::decition::DecideLine00::hasZhuched = false;
-
-double iv::decition::DecideLine00::lastLidarDisAvoid = -1;
-double iv::decition::DecideLine00::lidarDistanceAvoid = -1;
-
-int iv::decition::DecideLine00::finishPointNum = 0;
-int iv::decition::DecideLine00::zhuchePointNum = 0;
-
-
-
-
-
-//日常展示
-
-iv::decition::Decition    iv::decition::DecideLine00::getDecideFromGPS(GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, iv::LidarGridPtr lidarGridPtr,
-                                                                       std::vector<iv::Perception::PerceptionOutput> lidar_per) {
-
-	Decition gps_decition(new DecitionBasic);
-
-
-	if (isFirstRun)
-	{
-		vehState = normalRun;	
-		startAvoid_gps_ins = now_gps_ins;
-
-
-		ServiceCarStatus.carState = 1;
-		/*roadOri = gpsMapLine[PathPoint]->roadOri;
-		roadNow = roadOri;*/
-		busMode = false;
-
-
-       waitGpsTimeWidth= ServiceCarStatus.waitGpsTimeWidth;
-        outGarageLong=ServiceCarStatus.outGarageLong;
-		isFirstRun = false;
-	}
-    busMode=ServiceCarStatus.busmode;
-    ServiceCarStatus.useMobileEye=true;
-	gps_decition->leftlamp = false;
-	gps_decition->rightlamp = false;
-
-	
-	is_arrivaled = false;
-	
-
-
-	
-
-
-	
-
-	if (parkMode)
-	{
-
-		
-		handBrakePark(gps_decition,10000,now_gps_ins);
-		return gps_decition;
-	}
-
-
-	//驻车
-
-	if (zhucheMode)
-	{
-
-		if (trumpet()<5000)
-		{
-			gps_decition->brake=20;
-			gps_decition->accelerator = 0;
-			gps_decition->wheel_angle = 0;
-			gps_decition->speed = 0;
-			return gps_decition;
-		}
-		else
-		{
-			hasZhuched = true;
-			zhucheMode = false;
-			trumpetFirstCount = true;
-		}
-
-	}
-
-
-	//判断驻车标志位
-	if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
-	{
-		hasZhuched = false;
-	}
-
-
-
-	obsDistance = -1;
-    esrLineIndex = -1;
-	lidarDistance = -1;
-
-
-	roadPre = -1;
-
-
-
-    //realspeed = now_gps_ins.speed;
-    realspeed = ServiceCarStatus.vehSpeed_st;
-	secSpeed = realspeed / 3.6;
-
-
-    std::cout << "\n#############realspeed:\n" <<  realspeed << std::endl;
-
-	avoidX = (roadOri-roadNow)*roadWidth;
-
-	gpsTraceOri.clear();
-	gpsTraceNow.clear();
-	gpsTraceAvoid.clear();
-	gpsTraceBack.clear();
-
-
-    gpsTraceOri=getGpsTraceByMobileEye(ServiceCarStatus.Lane.curvature,ServiceCarStatus.Lane.heading,
-                                       (ServiceCarStatus.aftermarketLane.dist_to_lane_l+ServiceCarStatus.aftermarketLane.dist_to_lane_r)*0.5);
-     controlAng = iv::decition::Compute00().getDecideAngleByLanePID(realspeed);
-  //  controlAng = iv::decition::Compute00().getDecideAngle(gpsTraceOri, realspeed);
-    if(outGarageTimeLast!=-1){
-        outGarageTimeNow=GetTickCount();
-    }
-    if(outGarageTimeNow!=-1){
-        outGarageDistance+= (outGarageTimeNow-outGarageTimeLast)*secSpeed*0.001;
-         std::cout << "使用mobileEye决策————outGarageDistance:"<<outGarageDistance<< std::endl;
-    }
-    if(outGarageDistance>outGarageLong && !waitGps){
-        waitGps=true;
-
-        outGarageTimeLast=-1;
-        outGarageTimeNow=-1;
-        outGarageDistance=0;
-
-
-    }
-
-    if(ServiceCarStatus.m_bOutGarage && !waitGps){
-        outGarageTimeLast=GetTickCount();
-    }
-
-    if(waitGps){
-         std::cout << "使用mobileEye决策————waitGps"<< std::endl;
-        if(waitGpsTimeStart==-1){
-            waitGpsTimeStart=GetTickCount();
-        }
-        if(waitGpsTimeStart!=-1){
-            waitGpsTimeNow=GetTickCount();
-        }
-        if(waitGpsTimeNow!=-1){
-            waitGpsTimeSpan=waitGpsTimeNow-waitGpsTimeStart;
-            std::cout << "使用mobileEye决策————waitGps————waitGpsTimeSpan："<<waitGpsTimeSpan <<std::endl;
-        }
-        if(waitGpsTimeSpan>waitGpsTimeWidth){
-            waitGps=false;
-            waitGpsTimeStart=-1;
-            waitGpsTimeNow=-1;
-            waitGpsTimeSpan=0;
-             ServiceCarStatus.m_bOutGarage=false;
-               std::cout << "使用mobileEye决策————waitGps————退出"<< std::endl;
-        }
-
-        gps_decition->accelerator =0;
-        gps_decition->torque = 0;
-        gps_decition->brake=0.8;
-        gps_decition->wheel_angle = 0.0 - controlAng;
-        //           gps_decition->wheel_angle = 0;
-
-        return gps_decition;
-
-    }
-
-	
-
-
-
-	
-
-		
-
-  //  controlAng = iv::decition::Compute00().getDecideAngleByLane(realspeed);
-
-
-	if (readyParkMode)
-	{
-		double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
-
-	
-		if (parkDis < 25)
-		{
-			Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
-			double parkDistance = pt.y;
-
-
-			if (dSpeed > 15)
-			{
-				dSpeed = 15;
-			}
-
-
-			
-			if (parkDistance < 5 && parkDistance >= 4)
-			{
-                dSpeed = min(dSpeed, 8.0);
-			}
-
-
-
-			if (parkDistance < 1.5)
-			{
-				dSpeed = 0;
-				parkMode = true;
-				readyParkMode = false;
-			}
-
-
-		}
-	}
-
-
-
-	//准备驻车
-
-	if (readyZhucheMode)
-	{
-		double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
-
-		if (dis<25)
-		{
-			Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
-			double zhucheDistance = pt.y;
-
-
-			if (dSpeed > 15)
-			{
-				dSpeed = 15;
-			}
-
-
-
-			if (zhucheDistance < 5 && zhucheDistance >= 4)
-			{
-                dSpeed = min(dSpeed, 8.0);
-			}
-
-
-
-			if (zhucheDistance < 3)
-			{
-				dSpeed = 0;
-				zhucheMode = true;
-				readyZhucheMode = false;
-			}
-
-		}
-		
-		
-
-	}
-
-
-
-
-
-    dSpeed=15;
-
-
-	//速度结合角度的限制
-	controlAng = limitAngle(realspeed, controlAng);
-	dSpeed = limitSpeed(controlAng, dSpeed);
-
-	gps_decition->speed = dSpeed;
-
-
-	dSecSpeed = dSpeed / 3.6;
-	//	 ac = 2 * max(10, realspeed) / ((dSecSpeed*dSecSpeed) - (secSpeed*secSpeed));
-
-
-
-
-
-	/* if (obsDistance <0 && ac<0 && ac >= -0.5)
-	{
-	gps_decition->accelerator = 0;
-	gps_decition->brake = 0;
-	}
-	else {*/
-
-	
-
-
-
-
-
-
-
-
-        std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;
-		
-		//停车
-        if (ServiceCarStatus.carState == 0 && busMode)  //stop
-        {
-
-            gps_decition->accelerator =0;
-            gps_decition->torque = 0;
-            gps_decition->brake=0.5;
-            gps_decition->wheel_angle = 0.0 - controlAng;
-            //           gps_decition->wheel_angle = 0;
-
-            return gps_decition;
-
-        }
-
-
-
-        if (ServiceCarStatus.carState == 3 && busMode)   //  lin stop
-        {
-
-            ServiceCarStatus.carState=0;
-            gps_decition->accelerator = 0;
-            gps_decition->torque = 0;
-            gps_decition->brake=0.5;
-            gps_decition->wheel_angle = 0.0 - controlAng;
-            return gps_decition;
-
-
-        }
-
-
-
-
-
-
-
-        if (ServiceCarStatus.carState == 2 && busMode)  // dao zhan dian
-        {
-            dSpeed=3;
-        }
-
-
-
-
-                if (now_gps_ins.ins_status !=4 ) {
-
-                    dSpeed = min(15.0, dSpeed);
-
-                }
-
-
-        dSpeed=3;
-		dSecSpeed = dSpeed / 3.6;
-		gps_decition->speed = dSpeed;
-
-
-        decitionGpstool->computeObsOnRoad(lidarGridPtr,gpsTraceOri,0,gpsMapLine,lidar_per);
-
-        int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
-         int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
-         int conf=min(confL,confR);
-         if(conf<=1){
-
-         }
-
-        phaseSpeedDecition(gps_decition, secSpeed, decitionGpstool->obsDistance, decitionGpstool->obsSpeed,now_gps_ins);
-
-    gps_decition->wheel_angle = 0.0 - controlAng;
-
-    lastAngle=gps_decition->wheel_angle;
-
-	
-	return gps_decition;
-
-
-}
-
-
-
-void iv::decition::DecideLine00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDistance, double obsSpeed, GPS_INS gpsIns) {
-
-    //      obsDistance = -1;
-
-
-    std::cout << "\n计算用obs速度:%f\n" << obsSpeed << std::endl;
-    std::cout << "\n计算用obs距离:%f\n" << obsDistance << std::endl;
-
-
-
-    //  double acc = gpsIns.accel_y;
-    //    if(dSpeed>0)
-    //    dSpeed=dSpeed;
-    if(obsDistance<5 && obsDistance>0){
-        dSpeed=0;
-    }
-
-    dSecSpeed = dSpeed / 3.6;
-
-    double vt = dSecSpeed;
-    double vs = secSpeed;
-
-
-    if (abs(vs) < 0.05) vs = 0;
-    if (abs(obsSpeed) < 0.05) obsSpeed = 0;
-
-
-
-
-    double vl = vs + obsSpeed;
-    double vh = vs;
-    double dmax = 150;
-
-    //车距滤波
-    if (obsDistance < 0||obsDistance>100) {
-        obsDistance = 500;
-        obsSpeed=0;
-    }
-    if (obsDistance < 1) obsDistance = 1;
-
-    if (obsDistance > 150) vl = 25; //25m/s
-
-    if (obsSpeed <-15) obsDistance = obsDistance * 0.25 + lastDistance * 0.75;
-    else obsDistance = obsDistance * 0.5 + lastDistance * 0.5;
-
-
-
-    //TTC计算
-    double ds = 0.2566 * vl * vl + 5;
-    double ttcr = (vh - vl) / obsDistance;
-    double ttc = 15;
-    if (15 * (vh - vl) > obsDistance)
-        ttc = obsDistance / (vh - vl);
-
-
-    ServiceCarStatus.mfttc = ttc;
-
-
-    if (obsDistance <= dmax)
-    {
-        if (ttc > 10)
-        {
-            if (obsDistance > ds + 5)
-            {
-                double dds = min(30.0, obsDistance - (ds + 5));
-                vt = vt * dds / 30 + vl * (1 - dds / 30);
-            }
-            else
-            {
-                vt = min(vt, vl);
-            }
-        }
-        else
-        {
-            vt = min(vt, vl);
-        }
-    }
-
-    //   vt=min(vt,dSecSpeed);
-    vt=dSecSpeed;
-    std::cout << "\nvt:%f\n" << vt << std::endl;
-    //速度控制
-
-
-
-
-
-    double Id = 0;
-
-
-
-
-    double ed = ds - obsDistance;
-    if (obsDistance > 150) ed = 0;
-    double ev = vs - vt;
-    double u = 0;
-
-    if (ttc>10)  //if (ttc>10)
-    {
-
-        double kp = 0.5;        //double kp = 0.5;
-        double kd = 0.3;       //kd = 0.03
-        double k11 = 0.18;      //double k11 = 0.025;
-        double k12 = 0.000125;
-
-
-        double dev = (ev - lastEv) / 0.1;
-
-        Iv = 0.925 * Iv + ev;
-
-        Id = 0.85 * Id + ed;
-
-        if (abs(vh) < 2.0&& abs(vl) < 2.0)
-        {
-            Iv = 0.0; Id = 0.0;
-        }
-
-        /*u = kp * ev + kd * dev + k11 * Iv + k12 * Id;*/
-        u = kp * ev + kd * dev;
-    }
-
-
-    else if (ttc < 1.6 || (ttc <= 10 && obsDistance <= 0.75 * ds))
-    {
-        //AEB控制
-        Id = 0; Iv = 0;
-
-        u = 0;
-        if (ttc < 0.8) u = 7;
-        else if (ttc < 1.6) u = 3 + 5 * (1.6 - ttc);
-        else
-        {
-            u = (10 - ttc) * (10 - ttc) / 23.52;
-        }
-
-    }
-    else
-    {
-        //制动控制
-        Id = 0; Iv = 0;
-
-        if (obsDistance > 1.25 * ds)
-        {
-            double rev_ed = 1 / ed;
-
-            u = -0.5 * obsDistance * obsDistance * rev_ed * ttcr * ttcr;
-        }
-        else
-        {
-            if (abs(vl) > 2.0)
-                u = 0;
-            else
-                u = max(lastU, 1.0);
-        }
-    }
-
-    if (abs(vl) < 1.0 && abs(vh) < 1.0
-            && obsDistance < 2 * ds)
-    {
-        u = 0.5;
-    }
-
-
-
-
-
-
-    //u 限值
-    if (gpsIns.ins_pitch_angle>4&&u<-2.0)
-    {
-        u = -2.0;
-    }else if(ttc<3 && u<-0.2){
-        u=-0.2;
-    }
-    else
-    {
-
-        //     if (u < -0.7) u = -0.7;
-        if (u < -1.5) u = -1.5;
-    }
-
-    //        if (ttc > 8)
-    //        {
-    //            if (u > 0.8) u = 0.8;
-    //        }
-
-    if (u >= 0 && lastU <= 0)
-    {
-        if (u > 0.5) u = 0.5;
-    }
-    else if (u >= 0 && lastU >= 0)
-    {
-        if (u > lastU + 0.5) u = lastU + 0.5;
-    }
-    else if (u <= 0 && lastU >= 0)
-    {
-        if (u < -0.04) u = -0.04;
-    }
-    else if (u <= 0 && lastU <= 0)
-    {
-        if (u < lastU - 0.04) u = lastU - 0.04;
-    }
-
-
-
-
-    lastU = u;
-    lastEv = ev;
-    lastVt = vt;
-
-    //brakeValue start
-    if (u > 0)
-    {
-
-        decition->torque=0;
-        controlBrake=u; //102
-        if(obsDistance>0 && obsDistance<10){
-            controlBrake=u*1.5;
-        }
-        if(abs(dSpeed-realspeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0){
-            controlBrake=0;
-            decition->torque=max(0.0,ServiceCarStatus.torque_st-10.0);
-        }
-        //0110
-        if(changingDangWei){
-            controlBrake=max(0.3,controlBrake);
-        }
-
-    } //brakeValue end
-    else
-    {
-        controlBrake = 0;
-
-        if(ServiceCarStatus.torque_st==0){
-            decition->torque = (u*(-16)+((0-u)-ServiceCarStatus.mfVehAcc)*10)*0.7; //*1.0
-            decition->torque =max( decition->torque,1.0f);
-
-        }else{
-            decition->torque= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfVehAcc)*10;//1115    *10
-            }
-
-        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
-            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
-            if(realspeed<5  ){
-                decition->torque=min((float)25.0,decition->torque); //youmenxianxiao  30
-            }else if(realspeed<10){
-                decition->torque=min((float)25.0,decition->torque);  //40
-            }
-        }
-        decition->torque = min((float)(ServiceCarStatus.torque_st+5.0),decition->torque); //1115 5.0
-
-
-        // 斜坡加大油门   0217 lsn
-
-
-
-
-        if(((gpsIns.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-            (gpsIns.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-                && abs(realspeed)<1.0){
-            decition->torque=max((float)20.0,decition->torque);
-            decition->torque=min((float)40.0,decition->torque);
-        }
-
-
-
-       else if(((gpsIns.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
-            (gpsIns.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
-                && abs(realspeed)<10.0){
-                decition->torque=min((float)40.0,decition->torque); //youmenxianxiao  30
-        }
-
-
-
-
-        // 斜坡加大油门  end
-
-        decition->torque=min((float)100.0,decition->torque);
-        decition->torque=max((float)0.0,decition->torque);
-    }
-
-
-
-
-
-
-
-
-
-
-
-
-
-    //if (vt<2)                                                    //怠速
-    //{
-    //	controlSpeed = 0;
-    //}
-
-
-
-
-
-
-    //刹车限制
-
-    int BrakeIntMax = 10;
-    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
-    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
-
-    //`
-    if(ttc>10){
-        BrakeIntMax = 2;
-    }else if(ttc>6){
-        BrakeIntMax = 3;
-    }else if(ttc>3){
-        BrakeIntMax = 4;
-    }else if(ttc>1.6){
-        BrakeIntMax = 5;
-    }else if(ttc>0.8){
-        BrakeIntMax = 8;
-    }else{
-        BrakeIntMax = 10;
-    }
-    if(obsDistance>0 && obsDistance<10){
-        BrakeIntMax=max(BrakeIntMax,3);
-    }
-
-    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
-
-
-
-
-    //    if((obsDistance<0 || obsDistance>40)&&abs(dSpeed-realspeed)<3 && controlBrake>0){
-    //        controlBrake=0;
-    //        controlSpeed = max(ServiceCarStatus.torque_st-1.0,0.0); //1115
-    //    }
-    if((obsDistance<0 || obsDistance>40)&&abs(dSpeed-realspeed)<3 && controlBrake>0 && dSpeed>10){
-        controlBrake=0;
-        controlSpeed = max(ServiceCarStatus.torque_st-1.0,1.0); //1115
-    }
-
-
-
-    decition->brake = controlBrake;
-    decition->brake=min(100.0,double(decition->brake));
-    //   decition->torque = controlSpeed;
-
-    if (obsDistance>0)
-    {
-        iv::decition::DecideGps00::lastDistance = obsDistance;
-    }
-    else
-    {
-        iv::decition::DecideGps00::lastDistance = 200;
-    }
-
-
-
-    //1115
-
-    //        if(decition->torque>0){
-    //            if(decition->torque>lastTorque){
-    //                decition->torque=min(float(lastTorque+1.0),decition->torque);
-    //            }
-    ////            else if(decition->torque<lastTorque){
-    ////                decition->torque=max(float(lastTorque-5.0), decition->torque);
-    ////            }
-    //        }
-
-    //        lastTorque=decition->torque;
-
-    //斜坡刹车加大 lsn 0217
-    if (abs(gpsIns.ins_pitch_angle)>2.5 &&(decition->brake>0||dSpeed==0)){
-        decition->brake=max(2.0f,decition->brake);
-    }
-    //斜坡刹车加大 end
-
-
-    //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
-        decition->torque=0;
-        decition->brake=max(decition->brake,0.8f);
-    }
-
-
-
-
-
-    decition->accelerator=decition->torque;
-
-
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-double iv::decition::DecideLine00::limitAngle(double speed, double angle) {
-	double preAngle = angle;
-
-	if (speed > 15)
-	{
-		if (preAngle > 350)
-		{
-			preAngle = 350;
-		}
-		if (preAngle < -350)
-		{
-			preAngle = -350;
-		}
-	}
-	if (speed > 22)
-	{
-		if (preAngle > 200)
-		{
-			preAngle = 200;
-		}
-		if (preAngle < -200)
-		{
-			preAngle = -200;
-		}
-	}
-	if (speed > 25)
-	{
-		if (preAngle > 150)
-		{
-			preAngle = 150;
-		}
-		if (preAngle < -150)
-		{
-			preAngle = -150;
-		}
-	}
-	if (speed > 30)
-	{
-		if (preAngle > 70)
-		{
-			preAngle = 70;
-		}
-		if (preAngle < -70)
-		{
-			preAngle = -70;
-		}
-	}
-	if (speed > 45)                     //20
-	{
-		if (preAngle > 15)
-		{
-			preAngle = 15;
-		}
-		if (preAngle < -15)
-		{
-			preAngle = -15;
-		}
-	}
-	return preAngle;
-}
-
-
-
-double iv::decition::DecideLine00::limitSpeed(double angle, double speed) {
-
-	if (abs(angle) > 500 && speed > 8) speed = 8;
-	else if (abs(angle) > 350 && speed > 14) speed = 14;
-	else if (abs(angle) > 200 && speed > 21) speed = 21;
-	else if (abs(angle) > 150 && speed > 24) speed = 24;
-	else if (abs(angle) > 60 && speed > 29) speed = 29;
-	else if (abs(angle) > 20 && speed > 34) speed = 34;
-    return max(0.0, speed);
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-double iv::decition::DecideLine00::trumpet() {
-	if (trumpetFirstCount)
-	{
-		trumpetFirstCount = false;
-		trumpetLastTime= GetTickCount();
-        trumpetTimeSpan = 0.0;
-	}
-	else
-	{
-		trumpetStartTime= GetTickCount();
-		trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
-		trumpetLastTime = trumpetStartTime;
-	}
-	
-	return trumpetTimeSpan;
-}
-
-
-void  iv::decition::DecideLine00::handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins) {
-
-	if (abs(now_gps_ins.speed)>0.1)
-	{
-		decition->accelerator = 0;
-		decition->brake = 20;
-		decition->wheel_angle = 0;
-	
-	}
-	else
-	{
-
-		decition->accelerator = 0;
-		decition->brake = 20;
-		decition->wheel_angle = 0;
-		handPark = true;
-		handParkTime = duringTime;		
-	}
-
-}
-
-
-
-std::vector<iv::Point2D> iv::decition::DecideLine00::getGpsTraceByMobileEye(float a,float b, float c){
-    vector<iv::Point2D> trace;
-    for(int i=0; i<600;i++){
-       float y=0.1*i;
-       float x=(a*y*y+b*y+c);
-       iv::Point2D pt;
-        pt.x=x;
-        pt.y=y;
-        trace.push_back(pt);
-    }
-    return trace;
-}
-
+#include <decition/decide_line_00.h>
+#include <decition/compute_00.h>
+#include <decition/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/transfer.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <control/can.h>
+#include <time.h>
+using namespace std;
+
+#define PI (3.1415926535897932384626433832795)
+
+iv::decition::DecideLine00::DecideLine00() {
+      decitionGpstool = new iv::decition::DecideGps00();
+      mdecitionGpstool.reset(decitionGpstool);
+}
+iv::decition::DecideLine00::~DecideLine00() {
+
+}
+
+
+int iv::decition::DecideLine00::PathPoint = -1;
+double iv::decition::DecideLine00::minDis = iv::MaxValue;
+double iv::decition::DecideLine00::maxAngle = iv::MinValue;
+//int iv::decition::DecideLine00::lastGpsIndex = iv::MaxValue;
+int iv::decition::DecideLine00::lastGpsIndex = 0;
+double iv::decition::DecideLine00::controlSpeed = 0;
+double iv::decition::DecideLine00::controlBrake = 0;
+double iv::decition::DecideLine00::controlAng = 0;
+double iv::decition::DecideLine00::Iv = 0;
+double iv::decition::DecideLine00::lastU = 0;
+double iv::decition::DecideLine00::lastVt = 0;
+double iv::decition::DecideLine00::lastEv = 0;
+
+int iv::decition::DecideLine00::gpsLineParkIndex = 0;
+int iv::decition::DecideLine00::gpsMapParkIndex = 0;
+double iv::decition::DecideLine00::lastDistance = MaxValue;
+double iv::decition::DecideLine00::obsDistance = -1;
+double iv::decition::DecideLine00::obsDistanceAvoid = -1;
+double iv::decition::DecideLine00::lastDistanceAvoid = -1;
+
+double iv::decition::DecideLine00::lidarDistance = -1;
+double iv::decition::DecideLine00::myesrDistance = -1;
+double iv::decition::DecideLine00::lastLidarDis = -1;
+
+bool iv::decition::DecideLine00::parkMode = false;
+bool iv::decition::DecideLine00::readyParkMode = false;
+
+bool iv::decition::DecideLine00::zhucheMode = false;
+bool iv::decition::DecideLine00::readyZhucheMode = false;
+bool iv::decition::DecideLine00::hasZhuched = false;
+
+double iv::decition::DecideLine00::lastLidarDisAvoid = -1;
+double iv::decition::DecideLine00::lidarDistanceAvoid = -1;
+
+int iv::decition::DecideLine00::finishPointNum = 0;
+int iv::decition::DecideLine00::zhuchePointNum = 0;
+
+
+
+
+
+//日常展示
+
+iv::decition::Decition    iv::decition::DecideLine00::getDecideFromGPS(GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, iv::LidarGridPtr lidarGridPtr,
+                                                                       std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+	Decition gps_decition(new DecitionBasic);
+
+
+	if (isFirstRun)
+	{
+		vehState = normalRun;	
+		startAvoid_gps_ins = now_gps_ins;
+
+
+		ServiceCarStatus.carState = 1;
+		/*roadOri = gpsMapLine[PathPoint]->roadOri;
+		roadNow = roadOri;*/
+		busMode = false;
+
+
+       waitGpsTimeWidth= ServiceCarStatus.waitGpsTimeWidth;
+        outGarageLong=ServiceCarStatus.outGarageLong;
+		isFirstRun = false;
+	}
+    busMode=ServiceCarStatus.busmode;
+    ServiceCarStatus.useMobileEye=true;
+	gps_decition->leftlamp = false;
+	gps_decition->rightlamp = false;
+
+	
+	is_arrivaled = false;
+	
+
+
+	
+
+
+	
+
+	if (parkMode)
+	{
+
+		
+		handBrakePark(gps_decition,10000,now_gps_ins);
+		return gps_decition;
+	}
+
+
+	//驻车
+
+	if (zhucheMode)
+	{
+
+		if (trumpet()<5000)
+		{
+			gps_decition->brake=20;
+			gps_decition->accelerator = 0;
+			gps_decition->wheel_angle = 0;
+			gps_decition->speed = 0;
+			return gps_decition;
+		}
+		else
+		{
+			hasZhuched = true;
+			zhucheMode = false;
+			trumpetFirstCount = true;
+		}
+
+	}
+
+
+	//判断驻车标志位
+	if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
+	{
+		hasZhuched = false;
+	}
+
+
+
+	obsDistance = -1;
+    esrLineIndex = -1;
+	lidarDistance = -1;
+
+
+	roadPre = -1;
+
+
+
+    //realspeed = now_gps_ins.speed;
+    realspeed = ServiceCarStatus.vehSpeed_st;
+	secSpeed = realspeed / 3.6;
+
+
+    std::cout << "\n#############realspeed:\n" <<  realspeed << std::endl;
+
+	avoidX = (roadOri-roadNow)*roadWidth;
+
+	gpsTraceOri.clear();
+	gpsTraceNow.clear();
+	gpsTraceAvoid.clear();
+	gpsTraceBack.clear();
+
+
+    gpsTraceOri=getGpsTraceByMobileEye(ServiceCarStatus.Lane.curvature,ServiceCarStatus.Lane.heading,
+                                       (ServiceCarStatus.aftermarketLane.dist_to_lane_l+ServiceCarStatus.aftermarketLane.dist_to_lane_r)*0.5);
+     controlAng = iv::decition::Compute00().getDecideAngleByLanePID(realspeed);
+  //  controlAng = iv::decition::Compute00().getDecideAngle(gpsTraceOri, realspeed);
+    if(outGarageTimeLast!=-1){
+        outGarageTimeNow=GetTickCount();
+    }
+    if(outGarageTimeNow!=-1){
+        outGarageDistance+= (outGarageTimeNow-outGarageTimeLast)*secSpeed*0.001;
+         std::cout << "使用mobileEye决策————outGarageDistance:"<<outGarageDistance<< std::endl;
+    }
+    if(outGarageDistance>outGarageLong && !waitGps){
+        waitGps=true;
+
+        outGarageTimeLast=-1;
+        outGarageTimeNow=-1;
+        outGarageDistance=0;
+
+
+    }
+
+    if(ServiceCarStatus.m_bOutGarage && !waitGps){
+        outGarageTimeLast=GetTickCount();
+    }
+
+    if(waitGps){
+         std::cout << "使用mobileEye决策————waitGps"<< std::endl;
+        if(waitGpsTimeStart==-1){
+            waitGpsTimeStart=GetTickCount();
+        }
+        if(waitGpsTimeStart!=-1){
+            waitGpsTimeNow=GetTickCount();
+        }
+        if(waitGpsTimeNow!=-1){
+            waitGpsTimeSpan=waitGpsTimeNow-waitGpsTimeStart;
+            std::cout << "使用mobileEye决策————waitGps————waitGpsTimeSpan："<<waitGpsTimeSpan <<std::endl;
+        }
+        if(waitGpsTimeSpan>waitGpsTimeWidth){
+            waitGps=false;
+            waitGpsTimeStart=-1;
+            waitGpsTimeNow=-1;
+            waitGpsTimeSpan=0;
+             ServiceCarStatus.m_bOutGarage=false;
+               std::cout << "使用mobileEye决策————waitGps————退出"<< std::endl;
+        }
+
+        gps_decition->accelerator =0;
+        gps_decition->torque = 0;
+        gps_decition->brake=0.8;
+        gps_decition->wheel_angle = 0.0 - controlAng;
+        //           gps_decition->wheel_angle = 0;
+
+        return gps_decition;
+
+    }
+
+	
+
+
+
+	
+
+		
+
+  //  controlAng = iv::decition::Compute00().getDecideAngleByLane(realspeed);
+
+
+	if (readyParkMode)
+	{
+		double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
+
+	
+		if (parkDis < 25)
+		{
+			Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
+			double parkDistance = pt.y;
+
+
+			if (dSpeed > 15)
+			{
+				dSpeed = 15;
+			}
+
+
+			
+			if (parkDistance < 5 && parkDistance >= 4)
+			{
+                dSpeed = min(dSpeed, 8.0);
+			}
+
+
+
+			if (parkDistance < 1.5)
+			{
+				dSpeed = 0;
+				parkMode = true;
+				readyParkMode = false;
+			}
+
+
+		}
+	}
+
+
+
+	//准备驻车
+
+	if (readyZhucheMode)
+	{
+		double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
+
+		if (dis<25)
+		{
+			Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
+			double zhucheDistance = pt.y;
+
+
+			if (dSpeed > 15)
+			{
+				dSpeed = 15;
+			}
+
+
+
+			if (zhucheDistance < 5 && zhucheDistance >= 4)
+			{
+                dSpeed = min(dSpeed, 8.0);
+			}
+
+
+
+			if (zhucheDistance < 3)
+			{
+				dSpeed = 0;
+				zhucheMode = true;
+				readyZhucheMode = false;
+			}
+
+		}
+		
+		
+
+	}
+
+
+
+
+
+    dSpeed=15;
+
+
+	//速度结合角度的限制
+	controlAng = limitAngle(realspeed, controlAng);
+	dSpeed = limitSpeed(controlAng, dSpeed);
+
+	gps_decition->speed = dSpeed;
+
+
+	dSecSpeed = dSpeed / 3.6;
+	//	 ac = 2 * max(10, realspeed) / ((dSecSpeed*dSecSpeed) - (secSpeed*secSpeed));
+
+
+
+
+
+	/* if (obsDistance <0 && ac<0 && ac >= -0.5)
+	{
+	gps_decition->accelerator = 0;
+	gps_decition->brake = 0;
+	}
+	else {*/
+
+	
+
+
+
+
+
+
+
+
+        std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;
+		
+		//停车
+        if (ServiceCarStatus.carState == 0 && busMode)  //stop
+        {
+
+            gps_decition->accelerator =0;
+            gps_decition->torque = 0;
+            gps_decition->brake=0.5;
+            gps_decition->wheel_angle = 0.0 - controlAng;
+            //           gps_decition->wheel_angle = 0;
+
+            return gps_decition;
+
+        }
+
+
+
+        if (ServiceCarStatus.carState == 3 && busMode)   //  lin stop
+        {
+
+            ServiceCarStatus.carState=0;
+            gps_decition->accelerator = 0;
+            gps_decition->torque = 0;
+            gps_decition->brake=0.5;
+            gps_decition->wheel_angle = 0.0 - controlAng;
+            return gps_decition;
+
+
+        }
+
+
+
+
+
+
+
+        if (ServiceCarStatus.carState == 2 && busMode)  // dao zhan dian
+        {
+            dSpeed=3;
+        }
+
+
+
+
+                if (now_gps_ins.ins_status !=4 ) {
+
+                    dSpeed = min(15.0, dSpeed);
+
+                }
+
+
+        dSpeed=3;
+		dSecSpeed = dSpeed / 3.6;
+		gps_decition->speed = dSpeed;
+
+
+        decitionGpstool->computeObsOnRoad(lidarGridPtr,gpsTraceOri,0,gpsMapLine,lidar_per);
+
+        int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
+         int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
+         int conf=min(confL,confR);
+         if(conf<=1){
+
+         }
+
+        phaseSpeedDecition(gps_decition, secSpeed, decitionGpstool->obsDistance, decitionGpstool->obsSpeed,now_gps_ins);
+
+    gps_decition->wheel_angle = 0.0 - controlAng;
+
+    lastAngle=gps_decition->wheel_angle;
+
+	
+	return gps_decition;
+
+
+}
+
+
+
+void iv::decition::DecideLine00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDistance, double obsSpeed, GPS_INS gpsIns) {
+
+    //      obsDistance = -1;
+
+
+    std::cout << "\n计算用obs速度:%f\n" << obsSpeed << std::endl;
+    std::cout << "\n计算用obs距离:%f\n" << obsDistance << std::endl;
+
+
+
+    //  double acc = gpsIns.accel_y;
+    //    if(dSpeed>0)
+    //    dSpeed=dSpeed;
+    if(obsDistance<5 && obsDistance>0){
+        dSpeed=0;
+    }
+
+    dSecSpeed = dSpeed / 3.6;
+
+    double vt = dSecSpeed;
+    double vs = secSpeed;
+
+
+    if (abs(vs) < 0.05) vs = 0;
+    if (abs(obsSpeed) < 0.05) obsSpeed = 0;
+
+
+
+
+    double vl = vs + obsSpeed;
+    double vh = vs;
+    double dmax = 150;
+
+    //车距滤波
+    if (obsDistance < 0||obsDistance>100) {
+        obsDistance = 500;
+        obsSpeed=0;
+    }
+    if (obsDistance < 1) obsDistance = 1;
+
+    if (obsDistance > 150) vl = 25; //25m/s
+
+    if (obsSpeed <-15) obsDistance = obsDistance * 0.25 + lastDistance * 0.75;
+    else obsDistance = obsDistance * 0.5 + lastDistance * 0.5;
+
+
+
+    //TTC计算
+    double ds = 0.2566 * vl * vl + 5;
+    double ttcr = (vh - vl) / obsDistance;
+    double ttc = 15;
+    if (15 * (vh - vl) > obsDistance)
+        ttc = obsDistance / (vh - vl);
+
+
+    ServiceCarStatus.mfttc = ttc;
+
+
+    if (obsDistance <= dmax)
+    {
+        if (ttc > 10)
+        {
+            if (obsDistance > ds + 5)
+            {
+                double dds = min(30.0, obsDistance - (ds + 5));
+                vt = vt * dds / 30 + vl * (1 - dds / 30);
+            }
+            else
+            {
+                vt = min(vt, vl);
+            }
+        }
+        else
+        {
+            vt = min(vt, vl);
+        }
+    }
+
+    //   vt=min(vt,dSecSpeed);
+    vt=dSecSpeed;
+    std::cout << "\nvt:%f\n" << vt << std::endl;
+    //速度控制
+
+
+
+
+
+    double Id = 0;
+
+
+
+
+    double ed = ds - obsDistance;
+    if (obsDistance > 150) ed = 0;
+    double ev = vs - vt;
+    double u = 0;
+
+    if (ttc>10)  //if (ttc>10)
+    {
+
+        double kp = 0.5;        //double kp = 0.5;
+        double kd = 0.3;       //kd = 0.03
+        double k11 = 0.18;      //double k11 = 0.025;
+        double k12 = 0.000125;
+
+
+        double dev = (ev - lastEv) / 0.1;
+
+        Iv = 0.925 * Iv + ev;
+
+        Id = 0.85 * Id + ed;
+
+        if (abs(vh) < 2.0&& abs(vl) < 2.0)
+        {
+            Iv = 0.0; Id = 0.0;
+        }
+
+        /*u = kp * ev + kd * dev + k11 * Iv + k12 * Id;*/
+        u = kp * ev + kd * dev;
+    }
+
+
+    else if (ttc < 1.6 || (ttc <= 10 && obsDistance <= 0.75 * ds))
+    {
+        //AEB控制
+        Id = 0; Iv = 0;
+
+        u = 0;
+        if (ttc < 0.8) u = 7;
+        else if (ttc < 1.6) u = 3 + 5 * (1.6 - ttc);
+        else
+        {
+            u = (10 - ttc) * (10 - ttc) / 23.52;
+        }
+
+    }
+    else
+    {
+        //制动控制
+        Id = 0; Iv = 0;
+
+        if (obsDistance > 1.25 * ds)
+        {
+            double rev_ed = 1 / ed;
+
+            u = -0.5 * obsDistance * obsDistance * rev_ed * ttcr * ttcr;
+        }
+        else
+        {
+            if (abs(vl) > 2.0)
+                u = 0;
+            else
+                u = max(lastU, 1.0);
+        }
+    }
+
+    if (abs(vl) < 1.0 && abs(vh) < 1.0
+            && obsDistance < 2 * ds)
+    {
+        u = 0.5;
+    }
+
+
+
+
+
+
+    //u 限值
+    if (gpsIns.ins_pitch_angle>4&&u<-2.0)
+    {
+        u = -2.0;
+    }else if(ttc<3 && u<-0.2){
+        u=-0.2;
+    }
+    else
+    {
+
+        //     if (u < -0.7) u = -0.7;
+        if (u < -1.5) u = -1.5;
+    }
+
+    //        if (ttc > 8)
+    //        {
+    //            if (u > 0.8) u = 0.8;
+    //        }
+
+    if (u >= 0 && lastU <= 0)
+    {
+        if (u > 0.5) u = 0.5;
+    }
+    else if (u >= 0 && lastU >= 0)
+    {
+        if (u > lastU + 0.5) u = lastU + 0.5;
+    }
+    else if (u <= 0 && lastU >= 0)
+    {
+        if (u < -0.04) u = -0.04;
+    }
+    else if (u <= 0 && lastU <= 0)
+    {
+        if (u < lastU - 0.04) u = lastU - 0.04;
+    }
+
+
+
+
+    lastU = u;
+    lastEv = ev;
+    lastVt = vt;
+
+    //brakeValue start
+    if (u > 0)
+    {
+
+        decition->torque=0;
+        controlBrake=u; //102
+        if(obsDistance>0 && obsDistance<10){
+            controlBrake=u*1.5;
+        }
+        if(abs(dSpeed-realspeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0){
+            controlBrake=0;
+            decition->torque=max(0.0,ServiceCarStatus.torque_st-10.0);
+        }
+        //0110
+        if(changingDangWei){
+            controlBrake=max(0.3,controlBrake);
+        }
+
+    } //brakeValue end
+    else
+    {
+        controlBrake = 0;
+
+        if(ServiceCarStatus.torque_st==0){
+            decition->torque = (u*(-16)+((0-u)-ServiceCarStatus.mfVehAcc)*10)*0.7; //*1.0
+            decition->torque =max( decition->torque,1.0f);
+
+        }else{
+            decition->torque= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfVehAcc)*10;//1115    *10
+            }
+
+        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
+            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
+            if(realspeed<5  ){
+                decition->torque=min((float)25.0,decition->torque); //youmenxianxiao  30
+            }else if(realspeed<10){
+                decition->torque=min((float)25.0,decition->torque);  //40
+            }
+        }
+        decition->torque = min((float)(ServiceCarStatus.torque_st+5.0),decition->torque); //1115 5.0
+
+
+        // 斜坡加大油门   0217 lsn
+
+
+
+
+        if(((gpsIns.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+            (gpsIns.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realspeed)<1.0){
+            decition->torque=max((float)20.0,decition->torque);
+            decition->torque=min((float)40.0,decition->torque);
+        }
+
+
+
+       else if(((gpsIns.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+            (gpsIns.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realspeed)<10.0){
+                decition->torque=min((float)40.0,decition->torque); //youmenxianxiao  30
+        }
+
+
+
+
+        // 斜坡加大油门  end
+
+        decition->torque=min((float)100.0,decition->torque);
+        decition->torque=max((float)0.0,decition->torque);
+    }
+
+
+
+
+
+
+
+
+
+
+
+
+
+    //if (vt<2)                                                    //怠速
+    //{
+    //	controlSpeed = 0;
+    //}
+
+
+
+
+
+
+    //刹车限制
+
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else if(ttc>1.6){
+        BrakeIntMax = 5;
+    }else if(ttc>0.8){
+        BrakeIntMax = 8;
+    }else{
+        BrakeIntMax = 10;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    //    if((obsDistance<0 || obsDistance>40)&&abs(dSpeed-realspeed)<3 && controlBrake>0){
+    //        controlBrake=0;
+    //        controlSpeed = max(ServiceCarStatus.torque_st-1.0,0.0); //1115
+    //    }
+    if((obsDistance<0 || obsDistance>40)&&abs(dSpeed-realspeed)<3 && controlBrake>0 && dSpeed>10){
+        controlBrake=0;
+        controlSpeed = max(ServiceCarStatus.torque_st-1.0,1.0); //1115
+    }
+
+
+
+    decition->brake = controlBrake;
+    decition->brake=min(100.0,double(decition->brake));
+    //   decition->torque = controlSpeed;
+
+    if (obsDistance>0)
+    {
+        iv::decition::DecideGps00::lastDistance = obsDistance;
+    }
+    else
+    {
+        iv::decition::DecideGps00::lastDistance = 200;
+    }
+
+
+
+    //1115
+
+    //        if(decition->torque>0){
+    //            if(decition->torque>lastTorque){
+    //                decition->torque=min(float(lastTorque+1.0),decition->torque);
+    //            }
+    ////            else if(decition->torque<lastTorque){
+    ////                decition->torque=max(float(lastTorque-5.0), decition->torque);
+    ////            }
+    //        }
+
+    //        lastTorque=decition->torque;
+
+    //斜坡刹车加大 lsn 0217
+    if (abs(gpsIns.ins_pitch_angle)>2.5 &&(decition->brake>0||dSpeed==0)){
+        decition->brake=max(2.0f,decition->brake);
+    }
+    //斜坡刹车加大 end
+
+
+    //0227 10m nei xianzhi shache
+    if(obsDistance<10 &&obsDistance>0){
+        decition->torque=0;
+        decition->brake=max(decition->brake,0.8f);
+    }
+
+
+
+
+
+    decition->accelerator=decition->torque;
+
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+double iv::decition::DecideLine00::limitAngle(double speed, double angle) {
+	double preAngle = angle;
+
+	if (speed > 15)
+	{
+		if (preAngle > 350)
+		{
+			preAngle = 350;
+		}
+		if (preAngle < -350)
+		{
+			preAngle = -350;
+		}
+	}
+	if (speed > 22)
+	{
+		if (preAngle > 200)
+		{
+			preAngle = 200;
+		}
+		if (preAngle < -200)
+		{
+			preAngle = -200;
+		}
+	}
+	if (speed > 25)
+	{
+		if (preAngle > 150)
+		{
+			preAngle = 150;
+		}
+		if (preAngle < -150)
+		{
+			preAngle = -150;
+		}
+	}
+	if (speed > 30)
+	{
+		if (preAngle > 70)
+		{
+			preAngle = 70;
+		}
+		if (preAngle < -70)
+		{
+			preAngle = -70;
+		}
+	}
+	if (speed > 45)                     //20
+	{
+		if (preAngle > 15)
+		{
+			preAngle = 15;
+		}
+		if (preAngle < -15)
+		{
+			preAngle = -15;
+		}
+	}
+	return preAngle;
+}
+
+
+
+double iv::decition::DecideLine00::limitSpeed(double angle, double speed) {
+
+	if (abs(angle) > 500 && speed > 8) speed = 8;
+	else if (abs(angle) > 350 && speed > 14) speed = 14;
+	else if (abs(angle) > 200 && speed > 21) speed = 21;
+	else if (abs(angle) > 150 && speed > 24) speed = 24;
+	else if (abs(angle) > 60 && speed > 29) speed = 29;
+	else if (abs(angle) > 20 && speed > 34) speed = 34;
+    return max(0.0, speed);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+double iv::decition::DecideLine00::trumpet() {
+	if (trumpetFirstCount)
+	{
+		trumpetFirstCount = false;
+		trumpetLastTime= GetTickCount();
+        trumpetTimeSpan = 0.0;
+	}
+	else
+	{
+		trumpetStartTime= GetTickCount();
+		trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
+		trumpetLastTime = trumpetStartTime;
+	}
+	
+	return trumpetTimeSpan;
+}
+
+
+void  iv::decition::DecideLine00::handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins) {
+
+	if (abs(now_gps_ins.speed)>0.1)
+	{
+		decition->accelerator = 0;
+		decition->brake = 20;
+		decition->wheel_angle = 0;
+	
+	}
+	else
+	{
+
+		decition->accelerator = 0;
+		decition->brake = 20;
+		decition->wheel_angle = 0;
+		handPark = true;
+		handParkTime = duringTime;		
+	}
+
+}
+
+
+
+std::vector<iv::Point2D> iv::decition::DecideLine00::getGpsTraceByMobileEye(float a,float b, float c){
+    vector<iv::Point2D> trace;
+    for(int i=0; i<600;i++){
+       float y=0.1*i;
+       float x=(a*y*y+b*y+c);
+       iv::Point2D pt;
+        pt.x=x;
+        pt.y=y;
+        trace.push_back(pt);
+    }
+    return trace;
+}
+

src/decition/decition_brain_sf/decition/decition_maker.h

@@ -1,62 +1,62 @@
-#pragma once
-#ifndef _IV_DECITION_DECITION_MAKER_
-#define _IV_DECITION_DECITION_MAKER_
-
-#include <common_sf/gps_type.h>
-#include <decition/decition_type.h>
-#include<vector> 
-
-namespace iv {
-	namespace decition {
-		//根据传感器传输来的信息作出决策
-		class DecitionMaker
-		{
-		public:
-			DecitionMaker();
-			~DecitionMaker();
-
-            bool isFirstRun = true;                      //第一次找点标志
-            bool isComplete = false;
-			std::vector<iv::GPS_INS> vec_Gps_Group;
-			double angleDeviation;
-			double distanceDeviation;
-			double speedDeviation;
-			double origin_x;
-			double origin_y;
-			long lastNearPointNum = 0;
-			long objPointNum = 0;
-			GPS_INS startPoint;                          //起始位置最近点
-			GPS_INS nearestGpsIns;                       //最近车前路径点
-			GPS_INS nowGpsIns;                           //当前车位置点
-			GPS_INS objGpsIns;                           //跟踪目标点
-
-			float vehLenthAdj = 0;                       //实时定位点调整长度
-
-			GPS_INS getNearestGpsIns(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data);
-
-			iv::decition::Decition getDecideForGPS(iv::GPS_INS gpsIns, const std::vector<iv::GPSData> navigation_data);
-
-			void getStartPoint(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data);
-
-			void adjustOriginPoint(iv::GPS_INS gps_ins);
-
-			double getDistanceDeviation(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
-
-			double getCrossAngle(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
-
-			double getDirectCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns);
-
-			float getSpeedObj(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
-
-			void checkDistance(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data,double maxDistance);
-			
-            bool checkComplete(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data);
-
-		private:
-
-		};
-
-	}
-}
-
-#endif // !_IV_DECITION_DECITION_MAKER_
+#pragma once
+#ifndef _IV_DECITION_DECITION_MAKER_
+#define _IV_DECITION_DECITION_MAKER_
+
+#include <common_sf/gps_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+
+namespace iv {
+	namespace decition {
+		//根据传感器传输来的信息作出决策
+		class DecitionMaker
+		{
+		public:
+			DecitionMaker();
+			~DecitionMaker();
+
+            bool isFirstRun = true;                      //第一次找点标志
+            bool isComplete = false;
+			std::vector<iv::GPS_INS> vec_Gps_Group;
+			double angleDeviation;
+			double distanceDeviation;
+			double speedDeviation;
+			double origin_x;
+			double origin_y;
+			long lastNearPointNum = 0;
+			long objPointNum = 0;
+			GPS_INS startPoint;                          //起始位置最近点
+			GPS_INS nearestGpsIns;                       //最近车前路径点
+			GPS_INS nowGpsIns;                           //当前车位置点
+			GPS_INS objGpsIns;                           //跟踪目标点
+
+			float vehLenthAdj = 0;                       //实时定位点调整长度
+
+			GPS_INS getNearestGpsIns(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data);
+
+			iv::decition::Decition getDecideForGPS(iv::GPS_INS gpsIns, const std::vector<iv::GPSData> navigation_data);
+
+			void getStartPoint(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data);
+
+			void adjustOriginPoint(iv::GPS_INS gps_ins);
+
+			double getDistanceDeviation(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
+
+			double getCrossAngle(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
+
+			double getDirectCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns);
+
+			float getSpeedObj(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
+
+			void checkDistance(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data,double maxDistance);
+			
+            bool checkComplete(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data);
+
+		private:
+
+		};
+
+	}
+}
+
+#endif // !_IV_DECITION_DECITION_MAKER_

src/decition/decition_brain_sf/decition/decition_type.h

@@ -1,59 +1,59 @@
-#pragma once
-#ifndef _IV_DECITION_DECITION_TYPE_
-#define _IV_DECITION_DECITION_TYPE_
-#include <common_sf/boost.h>
-namespace iv {
-namespace decition {
-struct DecitionBasic {
-    float speed;				//车速
-    float wheel_angle;			//转向角度
-    float brake;				//刹车
-    float accelerator;			//油门
-    float torque;               //力矩
-    bool leftlamp;				//左转向灯
-    bool rightlamp;				//右转向灯
-
-    int  engine;
-    int  grade;
-    int  mode;
-    int handBrake;
-    bool speak;
-    bool door;
-    bool bright;
-    int dangWei;
-
-    float angSpeed;
-    int brakeType :1;
-    char brakeEnable;  //add by fjk
-    bool left;         //add by fjk
-    bool right;        //add by fjk
-
-    bool angleEnable;
-    bool speedEnable;
-    bool dangweiEnable;
-    int  driveMode;
-
-    int directLight;
-    int brakeLight;
-    int backLight;
-    int topLight;
-
-    int farLight;
-    int nearLight;
-
-    bool   air_enable ;                  //空调使能
-     bool   air_on;
-    float   air_temp ;                  //空调温度
-    float   air_mode ;                  //空调模式
-    float   wind_level ;                  //空调风力
-    float   roof_light ;                  //顶灯
-    float   home_light ;                  //日光灯
-    float   air_worktime ;                  //空调工作时间
-    float   air_offtime ;                  //空调关闭时间
-
-};
-typedef boost::shared_ptr<DecitionBasic> Decition;	//决策
-}
-}
-#endif // !_IV_DECITION_DECITION_TYPE_
-
+#pragma once
+#ifndef _IV_DECITION_DECITION_TYPE_
+#define _IV_DECITION_DECITION_TYPE_
+#include <common_sf/boost.h>
+namespace iv {
+namespace decition {
+struct DecitionBasic {
+    float speed;				//车速
+    float wheel_angle;			//转向角度
+    float brake;				//刹车
+    float accelerator;			//油门
+    float torque;               //力矩
+    bool leftlamp;				//左转向灯
+    bool rightlamp;				//右转向灯
+
+    int  engine;
+    int  grade;
+    int  mode;
+    int handBrake;
+    bool speak;
+    bool door;
+    bool bright;
+    int dangWei;
+
+    float angSpeed;
+    int brakeType :1;
+    char brakeEnable;  //add by fjk
+    bool left;         //add by fjk
+    bool right;        //add by fjk
+
+    bool angleEnable;
+    bool speedEnable;
+    bool dangweiEnable;
+    int  driveMode;
+
+    int directLight;
+    int brakeLight;
+    int backLight;
+    int topLight;
+
+    int farLight;
+    int nearLight;
+
+    bool   air_enable ;                  //空调使能
+     bool   air_on;
+    float   air_temp ;                  //空调温度
+    float   air_mode ;                  //空调模式
+    float   wind_level ;                  //空调风力
+    float   roof_light ;                  //顶灯
+    float   home_light ;                  //日光灯
+    float   air_worktime ;                  //空调工作时间
+    float   air_offtime ;                  //空调关闭时间
+
+};
+typedef boost::shared_ptr<DecitionBasic> Decition;	//决策
+}
+}
+#endif // !_IV_DECITION_DECITION_TYPE_
+

src/decition/decition_brain_sf/decition/decition_voter.cpp

@@ -1,22 +1,22 @@
-#include <decition/decition_voter.h>
-
-
-iv::decition::DecitionVoter::DecitionVoter()
-{
-}
-
-iv::decition::DecitionVoter::~DecitionVoter()
-{
-}
-
-void iv::decition::DecitionVoter::decideFromAll(iv::decition::Decition & decition_final, iv::decition::Decition & decition_lidar, iv::decition::Decition & decition_radar, iv::decition::Decition & decition_camera, iv::decition::Decition & decition_gps, iv::decition::Decition & decition_localmap)
-{
-    //todo 按照优先级决定采用谁的决策  或融合采用
-    if (decition_gps != NULL) {
-        decition_final->speed = decition_gps->speed;
-        decition_final->wheel_angle = decition_gps->wheel_angle;
-        decition_final->accelerator = decition_gps->accelerator;
-        decition_final->brake = decition_gps->brake;
-    }
-
-}
+#include <decition/decition_voter.h>
+
+
+iv::decition::DecitionVoter::DecitionVoter()
+{
+}
+
+iv::decition::DecitionVoter::~DecitionVoter()
+{
+}
+
+void iv::decition::DecitionVoter::decideFromAll(iv::decition::Decition & decition_final, iv::decition::Decition & decition_lidar, iv::decition::Decition & decition_radar, iv::decition::Decition & decition_camera, iv::decition::Decition & decition_gps, iv::decition::Decition & decition_localmap)
+{
+    //todo 按照优先级决定采用谁的决策  或融合采用
+    if (decition_gps != NULL) {
+        decition_final->speed = decition_gps->speed;
+        decition_final->wheel_angle = decition_gps->wheel_angle;
+        decition_final->accelerator = decition_gps->accelerator;
+        decition_final->brake = decition_gps->brake;
+    }
+
+}

src/decition/decition_brain_sf/decition/decition_voter.h

@@ -1,28 +1,28 @@
-#pragma once
-//对来自不同传感器所作出的决策进行加权判断  得出最终的决策（速度、角度)
-//障碍物信息-激光雷达
-//障碍物信息-毫米波雷达
-//障碍物信息-摄像头
-//局部地图规划出的路线
-//GPS 惯导和导航数据计算得出的路线
-#ifndef _IV_DECITION_VOTER_
-#define _IV_DECITION_VOTER_
-
-#include <decition/decition_type.h>
-
-namespace iv {
-	namespace decition {
-		class DecitionVoter
-		{
-		public:
-			DecitionVoter();
-			~DecitionVoter();
-
-			void decideFromAll(iv::decition::Decition & decition_final,iv::decition::Decition & decition_lidar, iv::decition::Decition & decition_radar, iv::decition::Decition & decition_camera, iv::decition::Decition & decition_gps, iv::decition::Decition & decition_localmap);
-
-		private:
-
-		};
-	}
-}
-#endif // !_IV_DECITION_VOTER_
+#pragma once
+//对来自不同传感器所作出的决策进行加权判断  得出最终的决策（速度、角度)
+//障碍物信息-激光雷达
+//障碍物信息-毫米波雷达
+//障碍物信息-摄像头
+//局部地图规划出的路线
+//GPS 惯导和导航数据计算得出的路线
+#ifndef _IV_DECITION_VOTER_
+#define _IV_DECITION_VOTER_
+
+#include <decition/decition_type.h>
+
+namespace iv {
+	namespace decition {
+		class DecitionVoter
+		{
+		public:
+			DecitionVoter();
+			~DecitionVoter();
+
+			void decideFromAll(iv::decition::Decition & decition_final,iv::decition::Decition & decition_lidar, iv::decition::Decition & decition_radar, iv::decition::Decition & decition_camera, iv::decition::Decition & decition_gps, iv::decition::Decition & decition_localmap);
+
+		private:
+
+		};
+	}
+}
+#endif // !_IV_DECITION_VOTER_

src/decition/decition_brain_sf/decition/fanyaapi.cpp

@@ -1,58 +1,58 @@
-#include "fanyaapi.h"
-#include <QMutex>
-
-QMutex gMutexDecison;
-qint64 gTimeDecision = 0;
-double gdecision[3];
-
-using namespace fanya;
-
-
-
-void ListenDecision(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    if(nSize < 3*sizeof(double))return;
-    gMutexDecison.lock();
-    memcpy(gdecision,strdata,3*sizeof(double));
-    gTimeDecision = QDateTime::currentMSecsSinceEpoch();
-    gMutexDecison.unlock();
-}
-
-fanyaapi::fanyaapi()
-{
-    mpa = iv::modulecomm::RegisterRecv("mpcdecision",ListenDecision);
-    mpb = iv::modulecomm::RegisterSend("GPS",1000,1);
-    mpc = iv::modulecomm::RegisterSend("MAP",10000000,1);
-    mpd = iv::modulecomm::RegisterSend("desiredspeed",1000,1);
-
-}
-
-int  fanyaapi::GetDecision(double &speed, double &decison, double &wheel)
-{
-    qint64 now = QDateTime::currentMSecsSinceEpoch();
-    if((now - gTimeDecision)> 1000)
-    {
-        return -1;
-    }
-    gMutexDecison.lock();
-    speed = gdecision[0];
-    decison = gdecision[1];
-    wheel = gdecision[2];
-    gMutexDecison.unlock();
-    return 0;
-}
-
-void fanyaapi::SetGPS(GPS_INS xgps)
-{
-    iv::modulecomm::ModuleSendMsg(mpb,(char *)&xgps,sizeof(GPS_INS));
-}
-
-void fanyaapi::SetMAP(std::vector<MAP_DATA> xvectorMAP)
-{
-    iv::modulecomm::ModuleSendMsg(mpc,(char *)xvectorMAP.data(),xvectorMAP.size() *sizeof(MAP_DATA));
-}
-
-void fanyaapi::SetDesiredspeed(double fspeed)
-{
-    iv::modulecomm::ModuleSendMsg(mpd,(char *)&fspeed,sizeof(fspeed));
-}
+#include "fanyaapi.h"
+#include <QMutex>
+
+QMutex gMutexDecison;
+qint64 gTimeDecision = 0;
+double gdecision[3];
+
+using namespace fanya;
+
+
+
+void ListenDecision(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize < 3*sizeof(double))return;
+    gMutexDecison.lock();
+    memcpy(gdecision,strdata,3*sizeof(double));
+    gTimeDecision = QDateTime::currentMSecsSinceEpoch();
+    gMutexDecison.unlock();
+}
+
+fanyaapi::fanyaapi()
+{
+    mpa = iv::modulecomm::RegisterRecv("mpcdecision",ListenDecision);
+    mpb = iv::modulecomm::RegisterSend("GPS",1000,1);
+    mpc = iv::modulecomm::RegisterSend("MAP",10000000,1);
+    mpd = iv::modulecomm::RegisterSend("desiredspeed",1000,1);
+
+}
+
+int  fanyaapi::GetDecision(double &speed, double &decison, double &wheel)
+{
+    qint64 now = QDateTime::currentMSecsSinceEpoch();
+    if((now - gTimeDecision)> 1000)
+    {
+        return -1;
+    }
+    gMutexDecison.lock();
+    speed = gdecision[0];
+    decison = gdecision[1];
+    wheel = gdecision[2];
+    gMutexDecison.unlock();
+    return 0;
+}
+
+void fanyaapi::SetGPS(GPS_INS xgps)
+{
+    iv::modulecomm::ModuleSendMsg(mpb,(char *)&xgps,sizeof(GPS_INS));
+}
+
+void fanyaapi::SetMAP(std::vector<MAP_DATA> xvectorMAP)
+{
+    iv::modulecomm::ModuleSendMsg(mpc,(char *)xvectorMAP.data(),xvectorMAP.size() *sizeof(MAP_DATA));
+}
+
+void fanyaapi::SetDesiredspeed(double fspeed)
+{
+    iv::modulecomm::ModuleSendMsg(mpd,(char *)&fspeed,sizeof(fspeed));
+}

src/decition/decition_brain_sf/decition/fanyaapi.h

@@ -1,38 +1,38 @@
-#ifndef FANYAAPI_H
-#define FANYAAPI_H
-
-#include <vector>
-#include "modulecomm.h"
-
-namespace fanya {
-struct GPS_INS        //惯导数据结构体
-{
-    double  gps_lat;  //纬度
-    double  gps_lng;  //经度
-    double  ins_heading; //航向
-    double  speed_y;   //纵向速度
-};
-struct MAP_DATA        //地图数据结构体
-{
-    double  gps_lat;  //纬度
-    double  gps_lng;  //经度
-    double  ins_heading; //航向
-};
-
-}
-
-
-class fanyaapi
-{
-public:
-    fanyaapi();
-    void SetGPS(fanya::GPS_INS xgps);
-    void SetMAP(std::vector<fanya::MAP_DATA> xvectorMAP);
-    void SetDesiredspeed(double fspeed);
-    int GetDecision(double & speed,double & decison, double & wheel);
-
-private:
-    void * mpa, * mpb, * mpc, * mpd;
-};
-
-#endif // FANYAAPI_H
+#ifndef FANYAAPI_H
+#define FANYAAPI_H
+
+#include <vector>
+#include "modulecomm.h"
+
+namespace fanya {
+struct GPS_INS        //惯导数据结构体
+{
+    double  gps_lat;  //纬度
+    double  gps_lng;  //经度
+    double  ins_heading; //航向
+    double  speed_y;   //纵向速度
+};
+struct MAP_DATA        //地图数据结构体
+{
+    double  gps_lat;  //纬度
+    double  gps_lng;  //经度
+    double  ins_heading; //航向
+};
+
+}
+
+
+class fanyaapi
+{
+public:
+    fanyaapi();
+    void SetGPS(fanya::GPS_INS xgps);
+    void SetMAP(std::vector<fanya::MAP_DATA> xvectorMAP);
+    void SetDesiredspeed(double fspeed);
+    int GetDecision(double & speed,double & decison, double & wheel);
+
+private:
+    void * mpa, * mpb, * mpc, * mpd;
+};
+
+#endif // FANYAAPI_H

src/decition/decition_brain_sf/decition/gnss_coordinate_convert.cpp

@@ -1,153 +1,153 @@
-#include <decition/gnss_coordinate_convert.h>
-
-void gps2xy(double J4, double K4, double *x, double *y)
-{
-    int L4 = (int)((K4 - 1.5) / 3) + 1;
-    double M4 = K4 - (L4 * 3);
-    double N4 = sin(J4 * 3.1415926536 / 180);
-    double O4 = cos(J4 * 3.1415926536 / 180);
-    double P4 = tan(J4 * 3.1415926536 / 180);
-    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
-    double R4 = sqrt(0.006738525414683) * O4;
-    double S4 = sqrt(1 + R4 * R4);
-    double T4 = 6399698.901783 / S4;
-    double U4 = (T4 / S4) / S4;
-    double V4 = O4 * M4 * 3.1415926536 / 180;
-    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
-    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
-    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
-    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
-
-    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
-    *x = 500000 + T4 * V4 * (Y4 + Z4);
-}
-/*
-//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
-void GaussProjCal(double longitude, double latitude, double *X, double *Y)
-{
-    int ProjNo = 0; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-    double a, f, e2, ee, NN, T, C, A, M, iPI;
-    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-    ZoneWide = 6; ////6度带宽
-    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-    ProjNo = (int)(longitude / ZoneWide);
-    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
-    longitude0 = longitude0 * iPI;
-    latitude0 = 0;
-    longitude1 = longitude * iPI; //经度转换为弧度
-    latitude1 = latitude * iPI; //纬度转换为弧度
-    e2 = 2 * f - f * f;
-    ee = e2 * (1.0 - e2);
-    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
-    T = tan(latitude1)*tan(latitude1);
-    C = ee * cos(latitude1)*cos(latitude1);
-    A = (longitude1 - longitude0)*cos(latitude1);
-    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
-        *e2 / 1024)*sin(2 * latitude1)
-        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
-    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
-    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
-        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
-    X0 = 1000000L * (ProjNo + 1) + 500000L;
-    Y0 = 0;
-    xval = xval + X0; yval = yval + Y0;
-    *X = xval;
-    *Y = yval;
-}
-*/
-
-
-//=======================zhaobo0904
-#define PI  3.14159265358979
-void GaussProjCal(double lon, double lat, double *X, double *Y)
-{
-    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
-    double a = 6378140.0;
-    double e2 = 0.006694384999588;
-    double ep2 = e2/(1-e2);
-
-    // 原点所在经度
-    double lon_origin = 6.0*int(lon/6) + 3.0;
-    lon_origin *= PI / 180.0;
-
-    double k0 = 0.9996;
-
-    // 角度转弧度
-    double lat1 = lat * PI / 180.0;
-    double lon1 = lon * PI / 180.0;
-
-
-    // 经线在该点处的曲率半径,
-    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
-
-
-    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
-    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
-    // 首先计算前四项的系数 a1~a4.
-    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
-    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
-    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
-    double a4 = (35*e2*e2*e2)/3072;
-    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
-
-    // 辅助量
-    double T = tan(lat1)*tan(lat1);
-    double C = ep2*cos(lat1)*cos(lat1);
-    double A = (lon1 - lon_origin)*cos(lat1);
-
-    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
-    *Y = M + N * tan(lat1) * ((A*A)/2 +
-                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
-                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
-
-
-    *Y *= k0;
-    return;
-}
-//==========================================================
-
-
-
-
-
-//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
-void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
-{
-    int ProjNo; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
-    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-    ZoneWide = 6; ////6度带宽
-    ProjNo = (int)(X / 1000000L); //查找带号
-    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
-    longitude0 = longitude0 * iPI; //中央经线
-    X0 = ProjNo * 1000000L + 500000L;
-    Y0 = 0;
-    xval = X - X0; yval = Y - Y0; //带内大地坐标
-    e2 = 2 * f - f * f;
-    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
-    ee = e2 / (1 - e2);
-    M = yval;
-    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
-    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
-                4 * u)
-            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
-    C = ee * cos(fai)*cos(fai);
-    T = tan(fai)*tan(fai);
-    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
-    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
-                                                                                       (fai)*sin(fai)));
-    D = xval / NN;
-    //计算经度(Longitude) 纬度(Latitude)
-    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
-                               *D*D*D*D / 120) / cos(fai);
-    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
-                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
-    //转换为度 DD
-    *longitude = longitude1 / iPI;
-    *latitude = latitude1 / iPI;
-}
+#include <decition/gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+/*
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+*/
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void GaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/decition/decition_brain_sf/decition/gnss_coordinate_convert.h

@@ -1,26 +1,26 @@
-#pragma once
-#ifndef _IV_PERCEPTION_GNSS_CONVERT_
-#define _IV_PERCEPTION_GNSS_CONVERT_
-
-#include <math.h>
-//double nmeaConvert2Lat(string lat)
-//{
-//	Console.WriteLine(lat);
-//	double nmea_d = double.Parse(lat.Substring(0, 2));
-//	double nmea_m = double.Parse(lat.Substring(2, 6));
-//	return nmea_d + nmea_m / 60;
-//}
-//
-//double nmeaConvert2Lon(string lon)
-//{
-//	Console.WriteLine(lon);
-//	double nmea_d = double.Parse(lon.Substring(0, 3));
-//	double nmea_m = double.Parse(lon.Substring(3, 7));
-//	return nmea_d + nmea_m / 60;
-//}
-
-void gps2xy(double , double , double *, double *);
-void GaussProjCal(double longitude, double latitude, double *X, double *Y);
-void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
-
-#endif // !_IV_PERCEPTION_GNSS_CONVERT_
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/decition/decition_brain_sf/decition/gps_distance.cpp

@@ -1,45 +1,45 @@
-#include <decition/gps_distance.h>
-#include <math.h>
-#define M_PI (3.1415926535897932384626433832795)
-
-// 计算弧度
-double iv::decition::rad(double d)
-{
-    const double PI = 3.1415926535898;
-    return d * PI / 180.0;
-}
-
-// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
-double iv::decition::CalcDistance(double fLati1, double fLong1, double fLati2, double fLong2)
-{
-    const float EARTH_RADIUS = 6378.137;
-    double radLat1 = rad(fLati1);
-    double radLat2 = rad(fLati2);
-    double a = radLat1 - radLat2;
-
-    double b = rad(fLong1) - rad(fLong2);
-    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));
-    s = s * EARTH_RADIUS;
-    //	s = (int)(s * 10000000) / 10000;
-    return s;
-}
-
-
-// 从直角坐标两点的直线距离，单位是米
-double iv::decition::DirectDistance(double x1, double y1, double x2, double y2)
-{
-    double s = sqrt(pow((x1-x2), 2) +pow((y1-y2), 2));
-    return s;
-}
-
-
-// 从直角坐标计算两点的夹角
-double iv::decition::DirectAngle(double x1, double y1, double x2, double y2)
-{
-    double angle = atan((y2 - y1) / (x2 - x1)) * 180 / M_PI;
-    return angle;
-}
-
-
-
-
+#include <decition/gps_distance.h>
+#include <math.h>
+#define M_PI (3.1415926535897932384626433832795)
+
+// 计算弧度
+double iv::decition::rad(double d)
+{
+    const double PI = 3.1415926535898;
+    return d * PI / 180.0;
+}
+
+// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
+double iv::decition::CalcDistance(double fLati1, double fLong1, double fLati2, double fLong2)
+{
+    const float EARTH_RADIUS = 6378.137;
+    double radLat1 = rad(fLati1);
+    double radLat2 = rad(fLati2);
+    double a = radLat1 - radLat2;
+
+    double b = rad(fLong1) - rad(fLong2);
+    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));
+    s = s * EARTH_RADIUS;
+    //	s = (int)(s * 10000000) / 10000;
+    return s;
+}
+
+
+// 从直角坐标两点的直线距离，单位是米
+double iv::decition::DirectDistance(double x1, double y1, double x2, double y2)
+{
+    double s = sqrt(pow((x1-x2), 2) +pow((y1-y2), 2));
+    return s;
+}
+
+
+// 从直角坐标计算两点的夹角
+double iv::decition::DirectAngle(double x1, double y1, double x2, double y2)
+{
+    double angle = atan((y2 - y1) / (x2 - x1)) * 180 / M_PI;
+    return angle;
+}
+
+
+
+

src/decition/decition_brain_sf/decition/gps_distance.h

@@ -1,26 +1,26 @@
-#pragma once
-#ifndef _IV_DECITION_GPS_DISTANCE_
-#define _IV_DECITION_GPS_DISTANCE_
-
-namespace iv {
-	namespace decition {
-        // 计算弧度
-		double rad(double d);
-
-		// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
-		double CalcDistance(double , double , double , double );
-
-		//计算直角坐标距离
-		double DirectDistance(double, double, double, double);
-
-		//计算直角坐标角度
-		double DirectAngle(double, double, double, double);
-
-
-	}
-}
-
-
-
-
-#endif // !_IV_DECITION_GPS_DISTANCE_
+#pragma once
+#ifndef _IV_DECITION_GPS_DISTANCE_
+#define _IV_DECITION_GPS_DISTANCE_
+
+namespace iv {
+	namespace decition {
+        // 计算弧度
+		double rad(double d);
+
+		// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
+		double CalcDistance(double , double , double , double );
+
+		//计算直角坐标距离
+		double DirectDistance(double, double, double, double);
+
+		//计算直角坐标角度
+		double DirectAngle(double, double, double, double);
+
+
+	}
+}
+
+
+
+
+#endif // !_IV_DECITION_GPS_DISTANCE_

src/decition/decition_brain_sf/decition/transfer.cpp

@@ -1,138 +1,138 @@
-#include <decition/transfer.h>
-#include <decition/decition_type.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-using namespace std;
-#define PI (3.1415926535897932384626433832795)
-
-
-
-///大地转车体
-iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
-{
-    double x_vehicle, y_vehicle;
-    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
-
-    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
-    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
-
-    x_vehicle = -cos(angle) * distance;
-    y_vehicle = sin(angle) * distance;
-    return  iv::Point2D(x_vehicle, y_vehicle);
-}
-
-///车体转大地
-iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
-{
-    iv::GPS_INS gps_ins;
-    double x_vehicle, y_vehicle;
-    double theta = atan2(x_path, y_path);
-
-    double distance = sqrt(x_path * x_path + y_path * y_path);
-    double angle = (pos.ins_heading_angle / 180 * PI + theta);
-
-    x_vehicle = pos.gps_x + distance * sin(angle);
-    y_vehicle = pos.gps_y + distance * cos(angle);
-    gps_ins.gps_x = x_vehicle;
-    gps_ins.gps_y = y_vehicle;
-
-    return  gps_ins;
-}
-
-double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
-{
-    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
-}
-
-double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
-{
-    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
-}
-
-
-
-
-//GPS转大地坐标
-void iv::decition::BLH2XYZ(iv::GPS_INS gp)
-{
-    int nFlag = 2;
-
-    double B = gp.gps_lat;
-
-    double L = gp.gps_lng;
-
-
-
-    double N, E, h;
-    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度
-
-    double a = 6378245.0;            //地球半径  北京6378245
-    double F = 298.257223563;        //地球扁率
-    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换
-
-    double f = 1 / F;
-    double b = a * (1 - f);
-    double ee = (a * a - b * b) / (a * a);
-    double e2 = (a * a - b * b) / (b * b);
-    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
-    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
-    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
-    double gm = 15 * n2 / 16 - 15 * n4 / 32;
-    double dt = -35 * n3 / 48 + 105 * n5 / 256;
-    double ep = 315 * n4 / 512;
-
-    B = B * iPI;
-    L = L * iPI;
-    L0 = L0 * iPI;
-
-    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
-    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
-    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
-    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
-    double yt = e2 * cos(B) * cos(B);
-    N = lB;
-    N += t * Nn * cl2 / 2;
-    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
-    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
-    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
-
-    E = Nn * cl;
-    E += Nn * cl3 * (1 - t2 + yt) / 6;
-    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
-    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
-
-    E += 500000;
-    if (nFlag == 1)
-    {
-        //UTM投影
-        N = 0.9996 * N;
-        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
-    }
-    if (nFlag == 2)
-    {
-        //UTM投影
-        N = 0.9999 * N;
-        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
-    }
-
-    //原
-    //pt2d.x = N;
-    //pt2d.y = E;
-    //
-    gp.gps_x = E - 280000;
-    gp.gps_y = N - 4325000;
-
-}
-
-
-double iv::decition::GetL0InDegree(double dLIn)
-{
-    //3°带求带号及中央子午线经度(d的形式)
-    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
-    double L = dLIn;//d.d
-    double L_ddd_Style = L;
-    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
-    double L0 = ZoneNumber * 3.0;//degree
-    return L0;
-}
+#include <decition/transfer.h>
+#include <decition/decition_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+using namespace std;
+#define PI (3.1415926535897932384626433832795)
+
+
+
+///大地转车体
+iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    double x_vehicle, y_vehicle;
+    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
+
+    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
+    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
+
+    x_vehicle = -cos(angle) * distance;
+    y_vehicle = sin(angle) * distance;
+    return  iv::Point2D(x_vehicle, y_vehicle);
+}
+
+///车体转大地
+iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    iv::GPS_INS gps_ins;
+    double x_vehicle, y_vehicle;
+    double theta = atan2(x_path, y_path);
+
+    double distance = sqrt(x_path * x_path + y_path * y_path);
+    double angle = (pos.ins_heading_angle / 180 * PI + theta);
+
+    x_vehicle = pos.gps_x + distance * sin(angle);
+    y_vehicle = pos.gps_y + distance * cos(angle);
+    gps_ins.gps_x = x_vehicle;
+    gps_ins.gps_y = y_vehicle;
+
+    return  gps_ins;
+}
+
+double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
+{
+    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
+}
+
+double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
+{
+    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
+}
+
+
+
+
+//GPS转大地坐标
+void iv::decition::BLH2XYZ(iv::GPS_INS gp)
+{
+    int nFlag = 2;
+
+    double B = gp.gps_lat;
+
+    double L = gp.gps_lng;
+
+
+
+    double N, E, h;
+    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度
+
+    double a = 6378245.0;            //地球半径  北京6378245
+    double F = 298.257223563;        //地球扁率
+    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换
+
+    double f = 1 / F;
+    double b = a * (1 - f);
+    double ee = (a * a - b * b) / (a * a);
+    double e2 = (a * a - b * b) / (b * b);
+    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
+    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
+    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
+    double gm = 15 * n2 / 16 - 15 * n4 / 32;
+    double dt = -35 * n3 / 48 + 105 * n5 / 256;
+    double ep = 315 * n4 / 512;
+
+    B = B * iPI;
+    L = L * iPI;
+    L0 = L0 * iPI;
+
+    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
+    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
+    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
+    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
+    double yt = e2 * cos(B) * cos(B);
+    N = lB;
+    N += t * Nn * cl2 / 2;
+    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
+    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
+    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
+
+    E = Nn * cl;
+    E += Nn * cl3 * (1 - t2 + yt) / 6;
+    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
+    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
+
+    E += 500000;
+    if (nFlag == 1)
+    {
+        //UTM投影
+        N = 0.9996 * N;
+        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
+    }
+    if (nFlag == 2)
+    {
+        //UTM投影
+        N = 0.9999 * N;
+        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
+    }
+
+    //原
+    //pt2d.x = N;
+    //pt2d.y = E;
+    //
+    gp.gps_x = E - 280000;
+    gp.gps_y = N - 4325000;
+
+}
+
+
+double iv::decition::GetL0InDegree(double dLIn)
+{
+    //3°带求带号及中央子午线经度(d的形式)
+    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
+    double L = dLIn;//d.d
+    double L_ddd_Style = L;
+    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
+    double L0 = ZoneNumber * 3.0;//degree
+    return L0;
+}

src/decition/decition_brain_sf/decition/transfer.h

@@ -1,27 +1,27 @@
-#pragma once
-#ifndef _IV_DECITION_TRANSFER_
-#define _IV_DECITION_TRANSFER_
-
-#include <common/gps_type.h>
-#include <decition/decition_type.h>
-#include<vector> 
-
-namespace iv {
-    namespace decition {
-        //根据传感器传输来的信息作出决策
-
-        double GetL0InDegree(double dLIn);
-
-        void BLH2XYZ(GPS_INS gps_ins);
-
-        ///大地转车体
-        Point2D Coordinate_Transfer(double x_path, double y_path, GPS_INS pos);
-
-        ///车体转大地
-        GPS_INS Coordinate_UnTransfer(double x_path, double y_path, GPS_INS pos);
-        double GetDistance(Point2D x1, Point2D x2);
-        double GetDistance(GPS_INS p1, GPS_INS p2);
-    }
-}
-
-#endif // ! _IV_DECITION_TRANSFER_
+#pragma once
+#ifndef _IV_DECITION_TRANSFER_
+#define _IV_DECITION_TRANSFER_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+
+namespace iv {
+    namespace decition {
+        //根据传感器传输来的信息作出决策
+
+        double GetL0InDegree(double dLIn);
+
+        void BLH2XYZ(GPS_INS gps_ins);
+
+        ///大地转车体
+        Point2D Coordinate_Transfer(double x_path, double y_path, GPS_INS pos);
+
+        ///车体转大地
+        GPS_INS Coordinate_UnTransfer(double x_path, double y_path, GPS_INS pos);
+        double GetDistance(Point2D x1, Point2D x2);
+        double GetDistance(GPS_INS p1, GPS_INS p2);
+    }
+}
+
+#endif // ! _IV_DECITION_TRANSFER_