C++ SensorData类代码示例

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();
		}
	}
}