int main(int ac, char *argv[]) {

  if (ac<2) {
    printf("\n UDP Receiver Illustration - by Kristof Van Laerhoven.");
    printf("\n syntax:");
    printf("\n   %s <portnr>", argv[0]);
    printf("\n");
    printf("\n  <portnr> is the port number to listen to");
    printf("\n try for instance '%s 1501'.", argv[0]);
    printf("\n\n");
    exit(0);
  }

  SensorData *s;
  
  // this is the only place where we treat the sensordata
  // as a specific UDP parser object, elsewhere, we don't
  // care where the sensordata came from. Neat, isn't it? 
  int port = atoi(argv[1]);
  s = new UDPParser(port, 100);
  
  char buffer[1024];
  
  while(1) {
  
    if (s->read(buffer))
        printf("msg: \"%s\"\n\r", buffer);
  }
  
  delete s;
   
 return 0;
}

void DBReader::mainLoop()
{
	SensorData data = this->getNextData();
	if(data.isValid())
	{
		if(!_odometryIgnored)
		{
			if(data.pose().isNull())
			{
				UWARN("Reading the database: odometry is null! "
					  "Please set \"Ignore odometry = true\" if there is "
					  "no odometry in the database.");
			}
			this->post(new OdometryEvent(data));
		}
		else
		{
			this->post(new CameraEvent(data));
		}

	}
	else if(!this->isKilled())
	{
		UINFO("no more images...");
		this->kill();
		this->post(new CameraEvent());
	}

}

int main(int ac, char *argv[]) {

  if (ac<2) {
    printf("\n Sensor Sim Illustration - by Kristof Van Laerhoven.");
    printf("\n syntax:");
    printf("\n   %s <nr>", argv[0]);
    printf("\n");
    printf("\n  <nr> is the number of sensors to simulate");
    printf("\n try for instance '%s 5'.", argv[0]);
    printf("\n\n");
    exit(0);
  }

  SensorData *s;
  
  // this is the only place where we treat the sensordata
  // as a specific sim parser object, elsewhere, we don't
  // care where the sensordata came from. Neat, isn't it? 
  int num = atoi(argv[1]);
  s = new SimParser(num);
  
  char buffer[1024];
  
  while(1) {
  
    if (s->read(buffer))
        printf("msg: \"%s\"\n\r", buffer);
  }
  
  delete s;
   
 return 0;
}

int main(void)
{
	CHR_6dm device("/dev/ttyUSB0");
	if (device.open(20) != CHR_OK) {
		fprintf(stderr, "Error Open");
		return -1;
	}
	printf("Open complete\n");

	SensorData sensor;
	sensor.enableChannel(az);
	sensor.enableChannel(ax);

	printf("Now set active channel\n");	
	if (device.setActiveChannels(&sensor) != CHR_OK)
	{
		fprintf(stderr, "Error Open");
		return -1;
	}

	printf("Now goto measurement mode\n");
	if (device.gotoMeasurementMode(&sensor, 300) != CHR_OK) {
		fprintf(stderr, "Error goto measurement mode\n");
		return -1;
	}

	for (int i=0; ; i++) {
		device.getData(&sensor);
		/*
		printf("Yaw:%f Pitch:%f Roll:%f\n", sensor.datafield.yaw(),
						    sensor.datafield.pitch(),
						    sensor.datafield.roll());
		*/
		printf("Acc_x:%3.4f Acc_y:%3.4f Acc_z:%3.4f\n", sensor.datafield.accel_x(),
						       sensor.datafield.accel_y(),
						       sensor.datafield.accel_z());
		printf("Mag_x:%3.4f Mag_y:%3.4f Mag_z:%3.4f\n", sensor.datafield.mag_x(),
						       sensor.datafield.mag_y(),
						       sensor.datafield.mag_z());
		printf("Gyro_x:%3.4f Gyro_y:%3.4f Gyro_z:%3.4f\n", sensor.datafield.gyro_x(),
						       sensor.datafield.gyro_y(),
						       sensor.datafield.gyro_z());
		usleep(100000);
	}

	printf("Go to config mode again!\n");
	if (device.gotoConfigMode() != CHR_OK) {
		fprintf(stderr, "Error goto config mode\n");
		return -1;
	}

	return 0;
}

void
Compass::OnDataReceived(SensorType sensorType, SensorData& sensorData, result r) {

	sensorData.GetValue((SensorDataKey)MAGNETIC_DATA_KEY_TIMESTAMP, timestamp);
	sensorData.GetValue((SensorDataKey)MAGNETIC_DATA_KEY_X, x);
	sensorData.GetValue((SensorDataKey)MAGNETIC_DATA_KEY_Y, y);
	sensorData.GetValue((SensorDataKey)MAGNETIC_DATA_KEY_Z, z);

	AppLogDebug("x: %f, y: %f, z: %f timestamp: %d", x, y, z, timestamp);

	String res;
	res.Format(256, L"PhoneGap.callbacks['%S'].success({x:%f,y:%f,z:%f,timestamp:%d});", callbackId.GetPointer(), x, y, z, timestamp);
	pWeb->EvaluateJavascriptN(res);
}

SensorData Camera::takeImage(CameraInfo * info)
{
	bool warnFrameRateTooHigh = false;
	float actualFrameRate = 0;
	if(_imageRate>0)
	{
		int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
		if(sleepTime > 2)
		{
			uSleep(sleepTime-2);
		}
		else if(sleepTime < 0)
		{
			warnFrameRateTooHigh = true;
			actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
		}

		// Add precision at the cost of a small overhead
		while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
		{
			//
		}

		double slept = _frameRateTimer->getElapsedTime();
		_frameRateTimer->start();
		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
	}

	UTimer timer;
	SensorData data  = this->captureImage(info);
	double captureTime = timer.ticks();
	if(warnFrameRateTooHigh)
	{
		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
				_imageRate, actualFrameRate, captureTime);
	}
	else
	{
		UDEBUG("Time capturing image = %fs", captureTime);
	}
	if(info)
	{
		info->id = data.id();
		info->stamp = data.stamp();
		info->timeCapture = captureTime;
	}
	return data;
}

Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
	if(_pose.isNull())
	{
		_pose.setIdentity(); // initialized
	}

	UASSERT(!data.image().empty());
	if(dynamic_cast<OdometryMono*>(this) == 0)
	{
		UASSERT(!data.depthOrRightImage().empty());
	}

	if(data.fx() <= 0 || data.fyOrBaseline() <= 0)
	{
		UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
				data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
		return Transform();
	}

	UTimer time;
	Transform t = this->computeTransform(data, info);

	if(info)
	{
		info->time = time.elapsed();
		info->lost = t.isNull();
	}

	if(!t.isNull())
	{
		_resetCurrentCount = _resetCountdown;

		if(_force2D)
		{
			float x,y,z, roll,pitch,yaw;
			t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
			t = Transform(x,y,0, 0,0,yaw);
		}

		return _pose *= t; // updated
	}
	else if(_resetCurrentCount > 0)
	{
		UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);

		--_resetCurrentCount;
		if(_resetCurrentCount == 0)
		{
			UWARN("Odometry automatically reset to latest pose!");
			this->reset(_pose);
		}
	}

	return Transform();
}

void
NotifySensorChange(const SensorData &aSensorData) {
  SensorObserverList &observers = GetSensorObservers(aSensorData.sensor());

  AssertMainThread();
  
  observers.Broadcast(aSensorData);
}

void
Accelerometer::OnDataReceived(SensorType sensorType, SensorData& sensorData, result r) {

	sensorData.GetValue((SensorDataKey)ACCELERATION_DATA_KEY_TIMESTAMP, timestamp);
	sensorData.GetValue((SensorDataKey)ACCELERATION_DATA_KEY_X, x);
	sensorData.GetValue((SensorDataKey)ACCELERATION_DATA_KEY_Y, y);
	sensorData.GetValue((SensorDataKey)ACCELERATION_DATA_KEY_Z, z);

	AppLogDebug("x: %f, y: %f, z: %f timestamp: %d", x, y, z, timestamp);

	String res;
	res.Format(256, L"PhoneGap.callbacks['%S'].success({x:%f,y:%f,z:%f,timestamp:%d});", callbackId.GetPointer(), x, y, z, timestamp);
	pWeb->EvaluateJavascriptN(res);

	res.Clear();
	res.Format(256, L"navigator.accelerometer.lastAcceleration = new Acceleration(%f,%f,%f,%d});", x, y, z, timestamp);
	pWeb->EvaluateJavascriptN(res);
}

	void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
	{
		UTimer timer;

		std::map<int, Transform> poses;
		std::multimap<int, Link> constraints;
		Transform mapOdom;
		rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
		for(unsigned int i=0; i<msg->nodes.size(); ++i)
		{
			if(msg->nodes[i].image.size() ||
			   msg->nodes[i].depth.size() ||
			   msg->nodes[i].laserScan.size())
			{
				uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i])));
			}
		}

		// create a tmp signature with latest sensory data
		if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
		{
			Signature tmpS = nodes_.at(poses.rbegin()->first);
			SensorData tmpData = tmpS.sensorData();
			tmpData.setId(-1);
			uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
			poses.insert(std::make_pair(-1, poses.rbegin()->second));
		}

		// Update maps
		poses = mapsManager_.updateMapCaches(
				poses,
				0,
				false,
				false,
				false,
				false,
				false,
				nodes_);

		mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);

		ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
	}

void PlatformSearch::wallFollow(SensorData data) {
  if (data.getDistanceB() < 15) {
    driveA->stop();
    driveB->stop();
    while (data.getDistanceB() < 30) {
      driveB->A->setSpeed(driveB->shield, 0.2);
      driveB->B->setSpeed(driveB->shield, -0.2);
      std::cout << "B" << std::endl;
    }
  } else if (data.getDistanceA() > 80) {
    driveA->A->setSpeed(driveB->shield, 0.2);
    driveA->B->setSpeed(driveB->shield, -0.2);
    usleep(300000);
  } else {
    driveA->drive(15, data.getDistanceA(), 0.2);
    std::cout << "A" << std::endl;
    usleep(100000);
  }
}

//============================================================
// MAIN LOOP
//============================================================
void RtabmapThread::process()
{
	SensorData data;
	getData(data);
	if(data.isValid())
	{
		if(_rtabmap->getMemory())
		{
			if(_rtabmap->process(data))
			{
				Statistics stats = _rtabmap->getStatistics();
				stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
				ULOGGER_DEBUG("posting statistics_ event...");
				this->post(new RtabmapEvent(stats));
			}
		}
		else
		{
			UERROR("RTAB-Map is not initialized! Ignoring received data...");
		}
	}
}

void CameraThread::mainLoop()
{
	UTimer totalTime;
	UDEBUG("");
	CameraInfo info;
	SensorData data = _camera->takeImage(&info);

	if(!data.imageRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
	{
		postUpdate(&data, &info);

		info.cameraName = _camera->getSerial();
		info.timeTotal = totalTime.ticks();
		this->post(new CameraEvent(data, info));
	}
	else if(!this->isKilled())
	{
		UWARN("no more images...");
		this->kill();
		this->post(new CameraEvent());
	}
}

 NS_IMETHOD Run()
 {
   SensorData sensorData;
   sensorData.sensor() = static_cast<SensorType>(mType);
   sensorData.timestamp() = PR_Now();
   sensorData.values().AppendElement(0.5f);
   sensorData.values().AppendElement(0.5f);
   sensorData.values().AppendElement(0.5f);
   sensorData.values().AppendElement(0.5f);
   sensorData.accuracy() = SENSOR_ACCURACY_UNRELIABLE;
   mTarget->Notify(sensorData);
   return NS_OK;
 }

void RtabmapThread::addData(const SensorData & sensorData)
{
	if(!_paused)
	{
		if(!sensorData.isValid())
		{
			ULOGGER_ERROR("data not valid !?");
			return;
		}

		if(_rate>0.0f)
		{
			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
			{
				return;
			}
		}
		_frameRateTimer->start();

		bool notify = true;
		_dataMutex.lock();
		{
			_dataBuffer.push_back(sensorData);
			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
			{
				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
				_dataBuffer.pop_front();
				notify = false;
			}
		}
		_dataMutex.unlock();

		if(notify)
		{
			_dataAdded.release();
		}
	}
}

//--------------------------------------------------------------
void testApp::onNewSensorData(SensorData & s){
  cout <<  s.toString() << endl;
  if (bUseSensors){
    float speed = 1.0*s.vitesse;
    float filterCoefSpeed = 0.00;
    wind_speed = speed * (1- filterCoefSpeed) + filterCoefSpeed*wind_speed;
    float direction;
    direction = 270 + s.direction;
    if (direction > 360) direction -= 360;
    if (wind.y <90 && direction > 270){
      direction -=360;
    }
    if (wind.y >270 && direction < 90){
      direction +=360;
    }

    float filterCoef = 0.90;
    wind.y = direction * (1- filterCoef) + filterCoef*wind.y;
    if (wind.y > 360) wind.y -= 360;
    temperature = s.temperature;
    pyranometre = s.pyranometre;
  }
}

void OdometryThread::addData(const SensorData & data)
{
	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryF2M*>(_odometry) == 0)
	{
		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
		{
			ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
			return;
		}
	}
	else
	{
		// Mono and BOW can accept RGB only
		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
		{
			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
			return;
		}
	}

	bool notify = true;
	_dataMutex.lock();
	{
		_dataBuffer.push_back(data);
		while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
		{
			UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
			_dataBuffer.pop_front();
			notify = false;
		}
	}
	_dataMutex.unlock();

	if(notify)
	{
		_dataAdded.release();
	}
}

SensorData CzmlReader::writeToSensorData(QJsonObject& data)
{
    QJsonArray positionArray;
    QJsonObject positionObject;
    SensorData sensordata;

    QString id = data["id"].toString();
    sensordata.setId(getIdFromCzmlString(id));

    positionObject = data["position"].toObject();
    positionArray = positionObject["cartographicDegrees"].toArray();
    QGeoCoordinate parsedPosition;
    parsedPosition.setLatitude(positionArray[1].toDouble());
    parsedPosition.setLongitude(positionArray[0].toDouble());
    sensordata.setPosition(parsedPosition);
    sensordata.setHeight(positionArray[2].toDouble());

    if(data["sensorvalue"].isString())
        sensordata.setSensorValue(std::numeric_limits<double>::min());
    else
        sensordata.setSensorValue(data["sensorvalue"].toDouble());

    return sensordata;
}

// return not null transform if odometry is correctly computed
Transform OdometryDVO::computeTransform(
		SensorData & data,
		const Transform & guess,
		OdometryInfo * info)
{
	Transform t;

#ifdef RTABMAP_DVO
	UTimer timer;

	if(data.imageRaw().empty() ||
		data.imageRaw().rows != data.depthOrRightRaw().rows ||
		data.imageRaw().cols != data.depthOrRightRaw().cols)
	{
		UERROR("Not supported input!");
		return t;
	}

	if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection()))
	{
		UERROR("Invalid camera model! Only single RGB-D camera supported by DVO. Try another odometry approach.");
		return t;
	}

	if(dvo_ == 0)
	{
		dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();

		dvo_ = new dvo::DenseTracker(cfg);
	}

	cv::Mat grey, grey_s16, depth_inpainted, depth_mask, depth_mono, depth_float;
	if(data.imageRaw().type() != CV_32FC1)
	{
		if(data.imageRaw().type() == CV_8UC3)
		{
			cv::cvtColor(data.imageRaw(), grey, CV_BGR2GRAY);
		}
		else
		{
			grey = data.imageRaw();
		}

		grey.convertTo(grey_s16, CV_32F);
	}
	else
	{
		grey_s16 = data.imageRaw();
	}

	// make sure all zeros are NAN
	if(data.depthRaw().type() == CV_32FC1)
	{
		depth_float = data.depthRaw();
		for(int i=0; i<depth_float.rows; ++i)
		{
			for(int j=0; j<depth_float.cols; ++j)
			{
				float & d = depth_float.at<float>(i,j);
				if(d == 0.0f)
				{
					d = NAN;
				}
			}
		}
	}
	else if(data.depthRaw().type() == CV_16UC1)
	{
		depth_float = cv::Mat(data.depthRaw().size(), CV_32FC1);
		for(int i=0; i<data.depthRaw().rows; ++i)
		{
			for(int j=0; j<data.depthRaw().cols; ++j)
			{
				float d = float(data.depthRaw().at<unsigned short>(i,j))/1000.0f;
				depth_float.at<float>(i, j) = d==0.0f?NAN:d;
			}
		}
	}
	else
	{
		UFATAL("Unknown depth format!");
	}

	if(camera_ == 0)
	{
		dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(
						data.cameraModels()[0].fx(),
						data.cameraModels()[0].fy(),
						data.cameraModels()[0].cx(),
						data.cameraModels()[0].cy());
		camera_ = new dvo::core::RgbdCameraPyramid(
				data.cameraModels()[0].imageWidth(),
				data.cameraModels()[0].imageHeight(),
				intrinsics);
	}

	dvo::core::RgbdImagePyramid * current = new dvo::core::RgbdImagePyramid(*camera_, grey_s16, depth_float);

	cv::Mat covariance;
//.........这里部分代码省略.........

void CameraThread::mainLoop()
{
	UTimer timer;
	UDEBUG("");
	SensorData data = _camera->takeImage();

	if(!data.imageRaw().empty())
	{
		if(_colorOnly && !data.depthRaw().empty())
		{
			data.setDepthOrRightRaw(cv::Mat());
		}
		if(_mirroring && data.cameraModels().size() == 1)
		{
			cv::Mat tmpRgb;
			cv::flip(data.imageRaw(), tmpRgb, 1);
			data.setImageRaw(tmpRgb);
			if(data.cameraModels()[0].cx())
			{
				CameraModel tmpModel(
						data.cameraModels()[0].fx(),
						data.cameraModels()[0].fy(),
						float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
						data.cameraModels()[0].cy(),
						data.cameraModels()[0].localTransform());
				data.setCameraModel(tmpModel);
			}
			if(!data.depthRaw().empty())
			{
				cv::Mat tmpDepth;
				cv::flip(data.depthRaw(), tmpDepth, 1);
				data.setDepthOrRightRaw(tmpDepth);
			}
		}
		if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
		{
			cv::Mat depth = util2d::depthFromDisparity(
					util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
					data.stereoCameraModel().left().fx(),
					data.stereoCameraModel().baseline());
			data.setCameraModel(data.stereoCameraModel().left());
			data.setDepthOrRightRaw(depth);
			data.setStereoCameraModel(StereoCameraModel());
		}

		this->post(new CameraEvent(data, _camera->getSerial()));
	}
	else if(!this->isKilled())
	{
		UWARN("no more images...");
		this->kill();
		this->post(new CameraEvent());
	}
}