#include #include iv::perception::Eyes::Eyes() { mgpsindex = 0; mgpsreadindex = 0; } iv::perception::Eyes::~Eyes() { delete sensor_gps; // delete sensor_camera; // delete sensor_radar; delete sensor_lidar; } 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(boost::bind(&iv::perception::Eyes::cb_lidar_grid, this, _1)); //sensor_radar->registerCallBack(boost::bind(&iv::perception::Eyes::cb_radar, this, _1)); //sensor_camera->registerCallBack(boost::bind(&iv::perception::Eyes::cb_camera, this, _1)); sensor_gps->registerCallBack(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; } } }