modularization/src/ui/ui_osgviewer/main.cpp

	/* 
 * esmini - Environment Simulator Minimalistic 
 * https://github.com/esmini/esmini
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at https://mozilla.org/MPL/2.0/.
 * 
 * Copyright (c) partners of Simulation Scenarios
 * https://sites.google.com/view/simulationscenarios
 */

 /*
  * The purpose of this application is to support development of the RoadManager by visualizing the road network and moving objects on top.
  * Bascially it loads an OpenDRIVE file, and optionally a corresponding 3D model, and then populate vehicles at specified density. The 
  * vehicles will simply follow it's lane until a potential junction where the choice of route is randomized.
  *
  * The application can be used both to debug the RoadManager and to check OpenDRIVE files, e.g. w.r.t. gemoetry, lanes and connectivity.
  *
  * New road/track segments is indicated by a yellow large dot. Geometry segments within a road are indicated by red dots.
  * Red line is the reference lane, blue lines shows drivable lanes. Non-drivable lanes are currently not indicated. 
  */

#include <random>
#include <iostream>
#define _USE_MATH_DEFINES
#include <math.h>

#include "viewer.hpp"
#include "RoadManager.hpp"
#include "CommonMini.hpp"
#include "alog.h"
#include "xmlparam.h"

#include <QMutex>

#include <QCoreApplication>

#include <objectarray.pb.h>


#define DEFAULT_SPEED   70  // km/h
#define DEFAULT_DENSITY 1   // Cars per 100 m
#define ROAD_MIN_LENGTH 30
#define SIGN(X) ((X<0)?-1:1)


static bool run_only_once = false;
static const double stepSize = 0.01;
static const double maxStepSize = 0.1;
static const double minStepSize = 0.01;
static const bool freerun = true;
static std::mt19937 mt_rand;
static double density = DEFAULT_DENSITY;
static double speed = DEFAULT_SPEED;
static double global_speed_factor = 1.0;
static int first_car_in_focus = -1;

double deltaSimTime;  // external - used by Viewer::RubberBandCamera

typedef struct
{
	int road_id_init;
	int lane_id_init;
	double heading_init;
	double s_init;
	roadmanager::Position *pos;
	double speed_factor;  // speed vary bewtween lanes, m/s
	viewer::EntityModel *model;
	int id;
} Car;

typedef struct
{
    double mrel_x;
    double mrel_y;
    viewer::EntityModel * model;
} RadarObj;

typedef struct
{
    double m_x;
    double m_y;
    double m_z;
    double mlength;
    double mwidth;
    double mheight;
    double mfYaw;
    viewer::EntityModel * model;
} LidarObj;

std::vector<Car*> cars;

std::vector<RadarObj *> gradarobj;
const int NRADARMX = 100;

Car * gADCIV_car;

viewer::EntityModel *  gtestRadar;

const int LIDARMAX = 300;
iv::lidar::objectarray glidar;
bool gbLidarupdate;
QMutex gMutexLidar;
std::vector<LidarObj *> glidarobj;

// Car models used for populating the road network
// path should be relative the OpenDRIVE file
static const char* carModelsFiles_[] =
{
	"car_white.osgb",
	"car_blue.osgb",
	"car_red.osgb",
	"car_yellow.osgb",
	"truck_yellow.osgb",
	"van_red.osgb",
	"bus_blue.osgb",
    "BMWZ4_10.hdr.ive",
};

std::vector<osg::ref_ptr<osg::LOD>> carModels_;



#include "modulecomm.h"
#include "gpsimu.pb.h"
#include "radarobjectarray.pb.h"
#include "gnss_coordinate_convert.h"

double glat0 = 39.0677643;//39.1201777;
double glon0 = 117.3548368;//117.02802270;

double gfrel_x = 0;
double gfrel_y = 0;
double gfrel_z = 0;
double gvehicle_hdg = 0;

double gvehicleheight = 1.6;//7.6;//1.6
double gfspeed = 0;

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;
        return;
    }
    (void)&index;
    (void)dt;
    (void)strmemname;

    double x0,y0;
    double x,y;
    double rel_x, rel_y;
    GaussProjCal(glon0,glat0,&x0,&y0);
    GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&x,&y);
    rel_x = x - x0;
    rel_y = y - y0;

    gfrel_x = rel_x;
    gfrel_y = rel_y;
    gfrel_z = xgpsimu.height();

    if(xgpsimu.has_speed())
    {
        gfspeed = xgpsimu.speed();
    }
    else
    {
        gfspeed = 3.6*sqrt(pow(xgpsimu.vn(),2)+pow(xgpsimu.ve(),2));
    }

    double hdg = xgpsimu.heading();
    hdg =  (90-hdg) * M_PI/180.0;
    if(hdg < 0)hdg = hdg + M_PI*2.0;
    gvehicle_hdg = hdg;
}

void ListenRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    static qint64 oldrecvtime;
    iv::radar::radarobjectarray xradararray;
    if(!xradararray.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenRadar Parse Error."<<std::endl;
        return;
    }

    int i;
    for(i=0;i<xradararray.obj_size();i++)
    {
        iv::radar::radarobject * pobj = xradararray.mutable_obj(i);
        if(i<gradarobj.size())
        {
            if(pobj->bvalid())
            {
            gradarobj.at(i)->mrel_x = pobj->x();
            gradarobj.at(i)->mrel_y = pobj->y();
            }
            else
            {
                gradarobj.at(i)->mrel_x = 10000;
                gradarobj.at(i)->mrel_y = 10000;
            }
        }
    }
    for(i=xradararray.obj_size();i<gradarobj.size();i++)
    {
        gradarobj.at(i)->mrel_x = 10000;
        gradarobj.at(i)->mrel_y = 10000;
    }
}

void Listenlidarcnndectect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

//    std::vector<iv::lidar::object>  lidarobjvec;
//    strtolidarobj(lidarobjvec,strdata,nSize);

    iv::lidar::objectarray lidarobjvec;
    std::string in;
    in.append(strdata,nSize);
    lidarobjvec.ParseFromString(in);
    gMutexLidar.lock();
    glidar.CopyFrom(lidarobjvec);
    gbLidarupdate = true;
    gMutexLidar.unlock();
//    qDebug("obj size is %d ",lidarobjvec.obj_size());
//    if(lidarobjvec.obj_size() > 1)
//    {
//        qDebug("type is %s",lidarobjvec.obj(0).type_name().data());
//    }
//    gw->UpdateLidarObj(lidarobjvec);
}



void log_callback(const char *str)
{
	printf("%s\n", str);
}


#include <osgDB/ReadFile>
#include <osg/ComputeBoundsVisitor>
#include <osg/LineWidth>
#include <osg/Point>
#include <osg/BlendFunc>
#include <osg/BlendColor>
#include <osg/Geode>
#include <osg/Group>
#include <osg/ShapeDrawable>
#include <osg/CullFace>
#include <osgGA/StateSetManipulator>
#include <osgGA/TrackballManipulator>
#include <osgGA/KeySwitchMatrixManipulator>
#include <osgGA/FlightManipulator>
#include <osgGA/DriveManipulator>
#include <osgGA/TerrainManipulator>
#include <osgGA/SphericalManipulator>
#include <osgViewer/ViewerEventHandlers>
#include <osgDB/Registry>
#include <osgShadow/StandardShadowMap>
#include <osgShadow/ShadowMap>
#include <osgShadow/ShadowedScene>
#include <osgUtil/SmoothingVisitor>
#include <osgUtil/Tessellator> // to tessellate multiple contours


void createosgbox()
{
    osg::ref_ptr<osg::Group> group = 0;
    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
    geode->addDrawable(new osg::ShapeDrawable(new osg::Box()));

    osg::ref_ptr<osg::Group> bbGroup = new osg::Group;
    osg::ref_ptr<osg::PositionAttitudeTransform> tx = new osg::PositionAttitudeTransform;

    osg::Material* material = new osg::Material();

    // Set color of vehicle based on its index
    double* color;
    double b = 1.5;  // brighness
    int index = 2;

    if (index == 0) color = color_white;
    else if (index == 1) color = color_red;
    else if (index == 2) color = color_blue;
    else color = color_yellow;

    material->setDiffuse(osg::Material::FRONT, osg::Vec4(b * color[0], b * color[1], b * color[2], 1.0));
    material->setAmbient(osg::Material::FRONT, osg::Vec4(b * color[0], b * color[1], b * color[2], 1.0));

    osg::ref_ptr<osg::Geode> geodeCopy = dynamic_cast<osg::Geode*>(geode->clone(osg::CopyOp::DEEP_COPY_ALL));
    group = new osg::Group;
    tx->addChild(geodeCopy);
    geodeCopy->setNodeMask(viewer::NodeMask::NODE_MASK_ENTITY_MODEL);
}

int SetupADCIVCar( viewer::Viewer *viewer)
{
//    int carModelID = 2;
    int carModelID = 7;
    Car *car_ = new Car;
    // Higher speeds in lanes closer to reference lane
    car_->speed_factor = 0.5 ;  // Speed vary between 0.5 to 1.0 times default speed
    car_->road_id_init = 0;
    car_->lane_id_init = 0;
    car_->s_init = 0;
 //   car_->pos = new roadmanager::Position(odrManager->GetRoadByIdx(r)->GetId(), lane->GetId(), s, 0);
 //   car_->pos->SetHeadingRelative(lane->GetId() < 0 ? 0 : M_PI);
 //   car_->heading_init = car_->pos->GetHRelative();

//    viewer->AddOSGBEnv("/home/nvidia/xiali_fangzhen.opt.osgb",osg::Vec3(1.0, 1.0, 1.0),"env");
//    viewer->AddOSGBEnv("/home/nvidia/xiali_fangzhen.shadow.osgb",osg::Vec3(0.5, 0.5, 0.5),"env1");
//    viewer->AddOSGBEnv("/home/nvidia/BMWZ4_10.hdr.ive",osg::Vec3(1.0, 1.0, 1.0),"env");

    if ((car_->model = viewer->AddEntityModel(carModelsFiles_[carModelID], osg::Vec3(0.5, 0.5, 0.5),
        viewer::EntityModel::EntityType::ENTITY_TYPE_VEHICLE, false, "", 0)) == 0)
    {
        return -1;
    }
    car_->id = 0;
    if (first_car_in_focus == -1 )
    {
        first_car_in_focus = car_->id;
    }
    gADCIV_car = car_;
    return 0;
}

int SetupRadar(viewer::Viewer *viewer)
{
    int i;
    for(i=0;i<NRADARMX;i++)
    {
        RadarObj * pRadar = new RadarObj;
        pRadar->mrel_x = 10000;
        pRadar->mrel_y = 10000;
        pRadar->model = viewer->AddRadarModel(osg::Vec3(0.5, 0.5, 0.5),"");
        gradarobj.push_back(pRadar);
    }
//    viewer->AddBackModel(osg::Vec3(0.5, 0.5, 0.5),"");


    return 0;
}

int SetupLidar(viewer::Viewer *viewer)
{
    int i;
    for(i=0;i<LIDARMAX;i++)
    {
        LidarObj * pLidar = new LidarObj;
        pLidar->m_x = 10000;
        pLidar->m_y = 10000;

        pLidar->model = viewer->AddLidarModel(osg::Vec3(0.5, 0.5, 0.5),"",0.01,0.01,0.01);
        glidarobj.push_back(pLidar);
    }
    return 0;
}

int SetupCars(roadmanager::OpenDrive *odrManager, viewer::Viewer *viewer)
{
	if (density < 1E-10)
	{
		// Basically no scenario vehicles
		return 0;
	}

//    for (int r = 0; r < 10; r++)
	for (int r = 0; r < odrManager->GetNumOfRoads(); r++)
	{
		roadmanager::Road *road = odrManager->GetRoadByIdx(r);
		double average_distance = 100.0 / density;

		if (road->GetLength() > ROAD_MIN_LENGTH)
		{
			// Populate road lanes with vehicles at some random distances
			for (double s = 10; s < road->GetLength() - average_distance; s += average_distance + 0.2 * average_distance * mt_rand() / mt_rand.max())
			{
				// Pick lane by random
				int lane_idx = ((double)road->GetNumberOfDrivingLanes(s) * mt_rand()) / (mt_rand.max)();
				roadmanager::Lane *lane = road->GetDrivingLaneByIdx(s, lane_idx);
				if (lane == 0)
				{
					LOG("Failed locate driving lane %d at s %d", lane_idx, s);
					continue;
				}


				if ((SIGN(lane->GetId()) < 0) && (road->GetLength() - s < 50) && (road->GetLink(roadmanager::LinkType::SUCCESSOR) == 0) ||
					(SIGN(lane->GetId()) > 0) && (s < 50) && (road->GetLink(roadmanager::LinkType::PREDECESSOR) == 0))
				{
					// Skip vehicles too close to road end - and where connecting road is missing
					continue;
				}

				// randomly choose model
				int carModelID = (double(sizeof(carModelsFiles_) / sizeof(carModelsFiles_[0])) * mt_rand()) / (mt_rand.max)();
				LOG("Adding car of model %d to road nr %d (road id %d s %.2f lane id %d), ", carModelID, r, road->GetId(), s, lane->GetId());

                carModelID = 2;

				Car *car_ = new Car;
				// Higher speeds in lanes closer to reference lane
				car_->speed_factor = 0.5 + 0.5 / abs(lane->GetId());  // Speed vary between 0.5 to 1.0 times default speed
				car_->road_id_init = odrManager->GetRoadByIdx(r)->GetId();
				car_->lane_id_init = lane->GetId();
				car_->s_init = s;
				car_->pos = new roadmanager::Position(odrManager->GetRoadByIdx(r)->GetId(), lane->GetId(), s, 0);
				car_->pos->SetHeadingRelative(lane->GetId() < 0 ? 0 : M_PI);
				car_->heading_init = car_->pos->GetHRelative();

				if ((car_->model = viewer->AddEntityModel(carModelsFiles_[carModelID], osg::Vec3(0.5, 0.5, 0.5), 
					viewer::EntityModel::EntityType::ENTITY_TYPE_VEHICLE, false, "", 0)) == 0)
				{
					return -1;
				}
				car_->id = cars.size();
				cars.push_back(car_);
				if (first_car_in_focus == -1 && lane->GetId() < 0)
				{
					first_car_in_focus = car_->id;
				}

			}
		}
	}

	if (first_car_in_focus == -1)
	{
		first_car_in_focus = 0;
	}

	return 0;
}

int SetupCarsSpecial(roadmanager::OpenDrive* odrManager, viewer::Viewer* viewer)
{
	// Setup one single vehicle in a dedicated pos
	Car* car_ = new Car;
	
	car_->speed_factor = 1.0;
	car_->road_id_init = 1;
	car_->lane_id_init = -1;
	car_->s_init = 40;
	car_->pos = new roadmanager::Position(car_->road_id_init, car_->lane_id_init, car_->s_init, 0);
	car_->pos->SetHeadingRelative(car_->lane_id_init < 0 ? 0 : M_PI);
	car_->heading_init = car_->pos->GetHRelative();

	if ((car_->model = viewer->AddEntityModel(carModelsFiles_[0], osg::Vec3(0.5, 0.5, 0.5),
		viewer::EntityModel::EntityType::ENTITY_TYPE_VEHICLE, false, "", 0)) == 0)
	{
		return -1;
	}
	car_->id = cars.size();
	cars.push_back(car_);

	first_car_in_focus = 0;

	return 0;
}


void updateADCIVCar(Car * car,viewer::Viewer *viewer)
{

    static float x=0;
    static float y=0;
    static float z=0;
    static float head = M_PI/2.0;
    static float pitch =0;
    static float roll = 0;


    if (car->model->txNode_ != 0)
    {
//        car->model->txNode_->setPosition(osg::Vec3(car->pos->GetX(), car->pos->GetY(), car->pos->GetZ()));

//        car->model->quat_.makeRotate(
//                    car->pos->GetR(), osg::Vec3(1, 0, 0),
//                    car->pos->GetP(), osg::Vec3(0, 1, 0),
//                    car->pos->GetH(), osg::Vec3(0, 0, 1));

        y = y+ 0.1;
        x = gfrel_x;
        y = gfrel_y;
        z = gfrel_z + gvehicleheight;
        head = gvehicle_hdg;
        car->model->txNode_->setPosition(osg::Vec3(x, y, z));

        car->model->quat_.makeRotate(
                    roll, osg::Vec3(1, 0, 0),
                    pitch, osg::Vec3(0, 1, 0),
                    head -M_PI/2.0, osg::Vec3(0, 0, 1));//改车头方向

        car->model->txNode_->setAttitude(car->model->quat_);


        int k;
 //       head = head +M_PI/2.0;
        for(k=0;k<gradarobj.size();k++)
        {
            double relx,rely;
            relx = gradarobj.at(k)->mrel_y*cos(head) + gradarobj.at(k)->mrel_x * sin(head);
            rely = gradarobj.at(k)->mrel_y*sin(head) - gradarobj.at(k)->mrel_x*cos(head);

            gradarobj.at(k)->model->txNode_->setPosition(osg::Vec3(x+relx, y+rely, z));

            gradarobj.at(k)->model->quat_.makeRotate(
                        roll, osg::Vec3(1, 0, 0),
                        pitch, osg::Vec3(0, 1, 0),
                        head, osg::Vec3(0, 0, 1));

            gradarobj.at(k)->model->txNode_->setAttitude(car->model->quat_);
        }

        bool bLidarUpdate = gbLidarupdate;
        if(bLidarUpdate)
        {
            gMutexLidar.lock();

            for(k=0;k<glidar.obj_size();k++)
            {
                iv::lidar::lidarobject * pobj = glidar.mutable_obj(k);
                if(k<LIDARMAX)
                {
                    glidarobj.at(k)->m_x = pobj->position().x();
                    glidarobj.at(k)->m_y = pobj->position().y();
                    glidarobj.at(k)->m_z = pobj->position().z();
                    glidarobj.at(k)->mlength = pobj->dimensions().x();
                    glidarobj.at(k)->mwidth = pobj->dimensions().y();
                    glidarobj.at(k)->mheight = pobj->dimensions().z();
                    glidarobj.at(k)->mfYaw = pobj->tyaw();
                }
            }
            for(k=glidar.obj_size();k<LIDARMAX;k++)
            {
                glidarobj.at(k)->m_x = 10000;
                glidarobj.at(k)->m_y = 10000;
                glidarobj.at(k)->m_z = 10000;
                glidarobj.at(k)->mlength = 0.01;
                glidarobj.at(k)->mwidth = 0.01;
                glidarobj.at(k)->mheight = 0.01;
            }
            gbLidarupdate = false;
            gMutexLidar.unlock();

            for(k=0;k<LIDARMAX;k++)
            {
                double rel_x,rel_y,rel_z;
//                rel_x = glidarobj.at(k)->m_x;
//                rel_y = glidarobj.at(k)->m_y;
                rel_z = glidarobj.at(k)->m_z;

                rel_x = glidarobj.at(k)->m_y*cos(head) + glidarobj.at(k)->m_x * sin(head);
                rel_y = glidarobj.at(k)->m_y*sin(head) - glidarobj.at(k)->m_x*cos(head);

                glidarobj.at(k)->m_x = x + rel_x;
                glidarobj.at(k)->m_y = y + rel_y;
                glidarobj.at(k)->m_z = z + rel_z;




//                if(glidarobj.at(k)->mlength>0.1)qDebug("len is %f width is %f ,height is %f",glidarobj.at(k)->mlength,glidarobj.at(k)->mwidth,glidarobj.at(k)->mheight);
                glidarobj.at(k)->model->ChangeScale(glidarobj.at(k)->mlength,glidarobj.at(k)->mwidth,glidarobj.at(k)->mheight);

                glidarobj.at(k)->model->txNode_->setPosition(osg::Vec3(glidarobj.at(k)->m_x, glidarobj.at(k)->m_y, glidarobj.at(k)->m_z));

                glidarobj.at(k)->model->quat_.makeRotate(
                            roll, osg::Vec3(1, 0, 0),
                            pitch, osg::Vec3(0, 1, 0),
                            head + glidarobj.at(k)->mfYaw, osg::Vec3(0, 0, 1));

                glidarobj.at(k)->model->txNode_->setAttitude(car->model->quat_);
            }

        }


    }

}


void updateCar(roadmanager::OpenDrive *odrManager, Car *car, double dt)
{
	double new_speed = car->pos->GetSpeedLimit() * car->speed_factor * global_speed_factor;
	double ds = new_speed * dt; // right lane is < 0 in road dir;

	if (car->pos->MoveAlongS(ds) != 0)
	{
		// Start from beginning of lane section - not initial s-position
		roadmanager::LaneSection* ls = odrManager->GetRoadById(car->road_id_init)->GetLaneSectionByS(car->s_init);
		double start_s = ls->GetS() + 5;
		if (car->lane_id_init > 0)
		{
			start_s = ls->GetS() + ls->GetLength() - 5;
		}
		car->pos->SetLanePos(car->road_id_init, car->lane_id_init, start_s, 0);
		car->pos->SetHeadingRelative(car->heading_init);
	}

	if (car->model->txNode_ != 0)
	{
		car->model->txNode_->setPosition(osg::Vec3(car->pos->GetX(), car->pos->GetY(), car->pos->GetZ()));

		car->model->quat_.makeRotate(
			car->pos->GetR(), osg::Vec3(1, 0, 0),
			car->pos->GetP(), osg::Vec3(0, 1, 0),
			car->pos->GetH(), osg::Vec3(0, 0, 1));

		car->model->txNode_->setAttitude(car->model->quat_);
	}
}

int main(int argcx, char** argvx)
{
	static char str_buf[128];
	SE_Options opt;

    int argc;
    char** argv;
    argc = 3;
    argv = new char*[3];
    argv[0] = "ui_osgviewer";
    argv[1] = "--odr";
    argv[2] = "/home/nvidia/map/map.xodr";

    std::string mappath = getenv("HOME");
//    mappath = mappath + "/map/mapa.xodr";
    mappath = mappath + "/map/map.xodr";
    argv[2] =  new char[256];
    strncpy(argv[2],mappath.c_str(),256);

    QString strpath = QCoreApplication::applicationDirPath();
//    QString apppath = strpath;
    if(argcx < 2)
        strpath = strpath + "/ui_osgviewer.xml";
    else
        strpath = argvx[1];
    std::cout<<strpath.toStdString()<<std::endl;
    iv::xmlparam::Xmlparam xp(strpath.toStdString());

    std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
    std::string strradarmsg = xp.GetParam("radarmsg","radar");
    std::string strcnnmsg = xp.GetParam("cnnmsg","lidar_track");
    std::string strheightajust = xp.GetParam("heightajust","-1.6");
    gvehicleheight = atof(strheightajust.data());


    void * pa = iv::modulecomm::RegisterRecv(strgpsmsg.data(),Listengpsimu);
    pa = iv::modulecomm::RegisterRecv(strradarmsg.data(),ListenRADAR);
    pa = iv::modulecomm::RegisterRecv(strcnnmsg.data(),Listenlidarcnndectect);
    (void)pa;

	// Use logger callback
	Logger::Inst().SetCallback(log_callback);

	mt_rand.seed((unsigned int)time(0));

	std::vector<std::string> args;
	for (int i = 0; i < argc; i++) args.push_back(argv[i]);

	// use an ArgumentParser object to manage the program arguments.
	opt.AddOption("odr", "OpenDRIVE filename", "odr_filename");
	opt.AddOption("model", "3D Model filename", "model_filename");
	opt.AddOption("density", "density (cars / 100 m)", "density");
	opt.AddOption("speed_factor", "speed_factor <number>", "speed_factor");
	opt.AddOption("osi_lines", "Show OSI road lines (toggle during simulation by press 'u') ");
	opt.AddOption("osi_points", "Show OSI road points (toggle during simulation by press 'y') ");
	opt.AddOption("road_features", "Show OpenDRIVE road features (toggle during simulation by press 'o') ");
	opt.AddOption("path", "Search path prefix for assets, e.g. car and sign model files", "path");
	opt.AddOption("logfile_path", "logfile path/filename, e.g. \"../esmini.log\" (default: log.txt)", "path");
	opt.AddOption("disable_log", "Prevent logfile from being created");
	opt.AddOption("help", "Show this help message");

    if (argc < 2 || opt.GetOptionSet("help"))
    {
        opt.PrintUsage();
        return -1;
    }

	opt.ParseArgs(&argc, argv);

	std::string arg_str;

	if (opt.GetOptionSet("disable_log"))
	{
		SE_Env::Inst().SetLogFilePath("");
		printf("Disable logfile\n");
	}
	else if (opt.IsOptionArgumentSet("logfile_path"))
	{
		arg_str = opt.GetOptionArg("logfile_path");
		SE_Env::Inst().SetLogFilePath(arg_str);
		if (arg_str.empty())
		{
			printf("Custom logfile path empty, disable logfile\n");
		}
		else
		{
			printf("Custom logfile path: %s\n", arg_str.c_str());
		}
	}
	Logger::Inst().OpenLogfile();

	if ((arg_str = opt.GetOptionArg("path")) != "")
	{
		SE_Env::Inst().AddPath(arg_str);
		LOG("Added path %s", arg_str.c_str());
	}



	std::string odrFilename = opt.GetOptionArg("odr");
 //   odrFilename = "/home/yuchuli/map/models/mapxiali.xodr";
	if (odrFilename.empty())
	{
		printf("Missing required argument --odr\n");
		opt.PrintUsage();
		return -1;
	}



	std::string modelFilename = opt.GetOptionArg("model");
	
	if (opt.GetOptionArg("density") != "")
	{
		density = strtod(opt.GetOptionArg("density"));
	}
	printf("density: %.2f\n", density);

	if (opt.GetOptionArg("speed_factor") != "")
	{
		global_speed_factor = strtod(opt.GetOptionArg("speed_factor"));
	}
	printf("global speed factor: %.2f\n", global_speed_factor);

	roadmanager::Position *lane_pos = new roadmanager::Position();
	roadmanager::Position *track_pos = new roadmanager::Position();

	try
	{
        if (!roadmanager::Position::LoadOpenDrive(odrFilename.c_str()))
        {
            printf("Failed to load ODR %s\n", odrFilename.c_str());
            return -1;
        }
		roadmanager::OpenDrive *odrManager = roadmanager::Position::GetOpenDrive();

		osg::ArgumentParser arguments(&argc, argv);
		viewer::Viewer *viewer = new viewer::Viewer(
			odrManager,
			modelFilename.c_str(),
			NULL,
			argv[0],
			arguments,
			&opt);

		viewer->SetWindowTitleFromArgs(args);
		
		if (opt.GetOptionSet("road_features"))
		{
			viewer->SetNodeMaskBits(viewer::NodeMask::NODE_MASK_ODR_FEATURES);
		}
		else
		{
			viewer->ClearNodeMaskBits(viewer::NodeMask::NODE_MASK_ODR_FEATURES);
		}

		if (opt.GetOptionSet("osi_lines"))
		{
			viewer->SetNodeMaskBits(viewer::NodeMask::NODE_MASK_OSI_LINES);
		}

		if (opt.GetOptionSet("osi_points"))
		{
			viewer->SetNodeMaskBits(viewer::NodeMask::NODE_MASK_OSI_POINTS);
		}

		printf("osi_features: lines %s points %s \n",
			viewer->GetNodeMaskBit(viewer::NodeMask::NODE_MASK_OSI_LINES) ? "on" : "off",
			viewer->GetNodeMaskBit(viewer::NodeMask::NODE_MASK_OSI_POINTS) ? "on" : "off");

         SetupADCIVCar(viewer);
         SetupRadar(viewer);
//         SetupLidar(viewer);
//         viewer->AddBackModel(osg::Vec3(0.5, 0.5, 0.5),"");
//         gtestRadar = viewer->AddRadarModel(osg::Vec3(0.5, 0.5, 0.5),"");
//         gtestRadar = viewer->AddEntityModel(carModelsFiles_[0], osg::Vec3(0.5, 0.5, 0.5),
//                                             viewer::EntityModel::EntityType::ENTITY_TYPE_VEHICLE, false, "", 0);
//        if (SetupCars(odrManager, viewer) == -1)
//        {
//            return 4;
//        }


		printf("%d cars added\n", (int)cars.size());
		viewer->SetVehicleInFocus(first_car_in_focus);

		__int64 now, lastTimeStamp = 0;

		static bool first_time = true;

		while (!viewer->osgViewer_->done())
		{
			// Get milliseconds since Jan 1 1970
			now = SE_getSystemTime();
			deltaSimTime = (now - lastTimeStamp) / 1000.0;  // step size in seconds
			lastTimeStamp = now;
			if (deltaSimTime > maxStepSize) // limit step size
			{
				deltaSimTime = maxStepSize;
			}
			else if (deltaSimTime < minStepSize)  // avoid CPU rush, sleep for a while
			{
				SE_sleep(now - lastTimeStamp);
				deltaSimTime = minStepSize;
			}

			if (!(run_only_once && !first_time))
			{

                updateADCIVCar(gADCIV_car,viewer);
                for (size_t i = 0; i < cars.size(); i++)
                {
                    updateCar(odrManager, cars[i], deltaSimTime);
                }
				first_time = false;
			}

            snprintf(str_buf, sizeof(str_buf), "(%6.2f,%6.2f,%6.2f,%6.2f) (%6.2fkm/h)", gfrel_x,
                gfrel_y,gfrel_z,gvehicle_hdg,gfspeed);
            viewer->SetInfoText(str_buf);
			// Set info text
			if (cars.size() > 0 && viewer->currentCarInFocus_ >= 0 && viewer->currentCarInFocus_ < cars.size())
			{
				Car* car = cars[viewer->currentCarInFocus_];
				snprintf(str_buf, sizeof(str_buf), "entity[%d]: %.2fkm/h (%d, %d, %.2f, %.2f) / (%.2f, %.2f %.2f)", viewer->currentCarInFocus_,
					3.6 * car->pos->GetSpeedLimit() * car->speed_factor * global_speed_factor, car->pos->GetTrackId(), car->pos->GetLaneId(),
					fabs(car->pos->GetOffset()) < SMALL_NUMBER ? 0 : car->pos->GetOffset(), car->pos->GetS(), car->pos->GetX(), car->pos->GetY(), car->pos->GetH());
				viewer->SetInfoText(str_buf);
			}

			// Step the simulation
			viewer->osgViewer_->frame();
		}
		delete viewer;
	}
	catch (std::logic_error &e)
	{
		printf("%s\n", e.what());
		return 2;
	}
	catch (std::runtime_error &e)
	{
		printf("%s\n", e.what());
		return 3;
	}

	for (size_t i = 0; i < cars.size(); i++)
	{
		delete(cars[i]);
	}
	delete track_pos;
	delete lane_pos;

	return 0;
}