bool
Output::shouldOutput()
{
// Do Nothing if output is not forced and allowing output has been disabled
if (!_force_output && !_allow_output)
return false;
// On initial step: between init() and first call to outputStep()
if (onInitial())
{
if (_force_output || _output_initial)
return true;
else
return false;
}
// On intermediate steps
else
{
// Do not perform output if:
// (1) Intermediate output is disabled
// (2) Output is not forced and (output is not allowed or not on interval)
if (!_output_intermediate || (!_force_output && (!_allow_output || !checkInterval())))
return false;
else
return true;
}
}
void
Output::outputFinal()
{
// If the intermediate steps are being output and the final time step is on an interval it will already have been output by outputStep, so do nothing
if (checkInterval() && _output_intermediate)
return;
// Do nothing if output is not forced or if output is disallowed
if (!_force_output && !_allow_output)
return;
// Do nothing if the output is not forced and final output is not desired
if (!_force_output && !_output_final)
return;
// If the mesh has changed or the sequence state is true, call the outputSetup() function
if (_mesh_changed || _sequence || !_output_setup_called)
{
outputSetup();
_output_setup_called = true;
}
// Perform the output
output();
// Set the force output flag to false
_force_output = false;
}
void
OutputBase::outputStep()
{
// Do nothing if output is not forced and is not allowed
if (!_force_output && !_allow_output)
return;
// Only continue if output is not forced and the output is on an inveval
if (!_force_output && !checkInterval())
return;
// If the mesh has changed or the sequence state is true or if it has not been called, call the outputSetup() function
if (_mesh_changed || _sequence || _num == 0 || !_output_setup_called)
{
// Execute the setup function
outputSetup();
// Reset the _mesh_changed flag
_mesh_changed = false;
// outputSetup has been called
_output_setup_called = true;
}
// Update the output number
_num++;
// Perform the output
output();
// Set the force output flag to false
_force_output = false;
}
void
Console::timestepSetup()
{
// Do nothing if the problem is steady or if it is not an output interval
if (!checkInterval())
return;
// Do nothing if output_initial = false and the timestep is zero
if (!_output_initial && _t_step == 0)
return;
// Stream to build the time step information
std::stringstream oss;
// Write timestep data for transient executioners
if (_transient)
{
// Get the length of the time step string
std::ostringstream time_step_string;
time_step_string << _t_step;
unsigned int n = time_step_string.str().size();
if (n < 2)
n = 2;
// Write time step and time information
oss << std::endl << "Time Step " << std::setw(n) << _t_step;
// Show scientific notation
if (_scientific_time)
oss << std::scientific;
// Print the time
oss << ", time = " << std::setw(9) << std::setprecision(9) << std::setfill('0') << std::showpoint << std::left << _time << std::endl;
// Show old time information, if desired
if (_verbose)
oss << std::setw(n) << " old time = " << std::setw(9) << std::setprecision(9) << std::setfill('0') << std::showpoint << std::left << _time_old << std::endl;
// Show the time delta information
oss << std::setw(2) << " dt = " << std::setw(9) << std::setprecision(9) << std::setfill('0') << std::showpoint << std::left << _dt << std::endl;
// Show the old time delta information, if desired
if (_verbose)
oss << std::setw(2) << " old dt = " << std::setw(9) << std::setprecision(9) << std::setfill('0') << std::showpoint << std::left << _dt_old << std::endl;
}
// Output to the screen
if (_write_screen)
Moose::out << oss.str();
// Output to the file
if (_write_file)
_file_output_stream << oss.str();
}
void
Console::timestepSetup()
{
// Do nothing if output is turned off
// Do nothing if the problem is steady or if it is not an output interval
// Do nothing if output_initial = false and the timestep is zero
if (!_allow_output || !checkInterval() || (!_output_initial && timeStep() == 0))
return;
// Stream to build the time step information
std::stringstream oss;
// Write timestep data for transient executioners
if (_transient)
{
// Get the length of the time step string
std::ostringstream time_step_string;
time_step_string << timeStep();
unsigned int n = time_step_string.str().size();
if (n < 2)
n = 2;
// Write time step and time information
oss << std::endl << "Time Step " << std::setw(n) << timeStep();
// Set precision
if (_precision > 0)
oss << std::setw(_precision) << std::setprecision(_precision) << std::setfill('0') << std::showpoint;
// Show scientific notation
if (_scientific_time)
oss << std::scientific;
// Print the time
oss << ", time = " << time() << std::endl;
// Show old time information, if desired
if (_verbose)
oss << std::right << std::setw(21) << std::setfill(' ') << "old time = " << std::left << timeOld() << '\n';
// Show the time delta information
oss << std::right << std::setw(21) << std::setfill(' ') << "dt = "<< std::left << dt() << '\n';
// Show the old time delta information, if desired
if (_verbose)
oss << std::right << std::setw(21) << std::setfill(' ') << "old dt = " << _dt_old << '\n';
}
// Output to the screen
write(oss.str());
}
void zTCPTask::checkSignal(const zRTime &ct)
{
///Zebra::logger->debug("zTCPTask::checkSignal");
if (ifCheckSignal() && checkInterval(ct))
{
if (checkTick())
{
//测试信号在指定时间范围内没有返回
Zebra::logger->error("套接口检查测试信号失败");
Terminate(zTCPTask::terminate_active);
}
else
{
//发送测试信号
Cmd::t_NullCmd tNullCmd;
//Zebra::logger->debug("服务端发送测试信号");
if (sendCmd(&tNullCmd,sizeof(tNullCmd)))
setTick();
}
}
}
int main(int argc , char** argv)
{
ros::init(argc, argv, "main");
std::vector<gps_data> gps;
std::vector<imu_data> imu;
ros::Duration duration(10.0);
getData(duration, gps, imu);
/*
int i = 0;
while (gps.size()>0 && imu.size()>0){
printf("gps TIME: %f\nx: %f, y: %f, z: %f\nimu TIME: %f\nvx: %f, vy: %f, vz: %f\nax:%f, ay: %f, az: %f",
gps[i].time, gps[i].x, gps[i].y, gps[i].z,
imu[i].time, imu[i].ang_vel_x, imu[i].ang_vel_y, imu[i].ang_vel_z, imu[i].acc_x, imu[i].acc_y, imu[i].acc_z);
gps.pop_back();
imu.pop_back();
}
*/
// Check if received enough sensor data
if (gps.size() < MINMUM_DATA_NUM){
std::cout << "Not enough data for optimization!" <<std::endl;
return 1;
}
// Initialize spline
size_t num_knots = 20;
double offset = gps[0].time;
double dt = 0.5;
std::vector<Pose> parameter_blocks;
for (size_t i=0; i < num_knots; ++i){
Eigen::Matrix<double, 6, 1> vec;
vec <<0, 0, 0, gps[0].x, gps[0].y, gps[0].z;
Pose p = Pose(vec);
parameter_blocks.push_back(p);
}
// Initialize the solver
double lambda = 0.001;
double iteration_max = 10;
lma::Solver<GPSConstraint, IMUConstraint> solver(lambda, iteration_max);
// Add constraint
for(size_t i = 0 ; i < gps.size() ; ++i){
int i0 = checkInterval(num_knots, offset, dt, gps[i].time);
if (i0 == -1)
continue;
else{
solver.add(GPSConstraint(gps[i], dt, offset+i0*dt, 1.0),¶meter_blocks[i0-1], ¶meter_blocks[i0], ¶meter_blocks[i0+1], ¶meter_blocks[i0+2] );
}
}
for(size_t i = 0 ; i < imu.size() ; i+=10){
int i0 = checkInterval(num_knots, offset, dt, imu[i].time);
if (i0 == -1)
continue;
else{
solver.add(IMUConstraint(imu[i], dt, offset+i0*dt, 1.0),¶meter_blocks[i0-1], ¶meter_blocks[i0], ¶meter_blocks[i0+1], ¶meter_blocks[i0+2] );
}
}
solver.solve(lma::DENSE,lma::enable_verbose_output());
// Output info
/*
for (size_t i = 0; i < parameter_blocks.size(); ++i){
std::cout <<parameter_blocks[i].data[4] <<
parameter_blocks[i].data[5] <<
parameter_blocks[i].data[6] << endl;
}
*/
return 0;
}
int sageBlockStreamer::streamLoop()
{
while (streamerOn) {
//int syncFrame = 0;
//sage::printLog("\n========= wait for a frame ========\n");
sageBlockFrame *buf = (sageBlockFrame *)doubleBuf->getBackBuffer();
//sage::printLog("\n========= got a frame ==========\n");
/* sungwon experimental */
if ( affinityFlag ) {
#if ! defined (__APPLE__)
cpu_set_t cpuset;
CPU_ZERO(&cpuset);
pthread_mutex_lock(&affinityMutex);
std::list<int>::iterator it;
for ( it=cpulist.begin(); it!=cpulist.end(); it++) {
CPU_SET((*it), &cpuset);
}
affinityFlag = false; // reset flag
pthread_mutex_unlock(&affinityMutex);
if ( pthread_setaffinity_np(pthread_self(), sizeof(cpuset), &cpuset) != 0 ) {
perror("\n\npthread_setaffinity_np\n");
}
if ( pthread_getaffinity_np(pthread_self(), sizeof(cpuset), &cpuset) != 0 ) {
perror("pthread_getaffinity_np");
}
else {
fprintf(stderr,"SBS::%s() : current CPU affinity : ", __FUNCTION__);
for (int i=0; i<CPU_SETSIZE; i++) {
if (CPU_ISSET(i, &cpuset)) {
fprintf(stderr, "%d ", i);
}
}
fprintf(stderr,"\n");
}
#endif
}
if ( config.swexp ) {
//buf->updateBufferHeader(frameID, config.resX, config.resY);
if ( nwObj->sendpixelonly(0, buf) <= 0 ) {
streamerOn = false;
}
else {
//fprintf(stderr, "sageBlockStreamer::%s() : frame %d sent \n", __FUNCTION__, frameID);
}
doubleBuf->releaseBackBuffer();
frameID++;
frameCounter++;
continue;
}
char *msgStr = NULL;
if (config.nodeNum > 1) {
config.syncClientObj->sendSlaveUpdate(frameID);
//sage::printLog("send update %d", config.rank);
config.syncClientObj->waitForSyncData(msgStr);
//sage::printLog("receive sync %d", config.rank);
if (msgStr) {
//std::cout << "reconfigure " << msgStr << std::endl;
reconfigureStreams(msgStr);
//firstConfiguration = false;
}
}
else {
pthread_mutex_lock(reconfigMutex);
if (msgQueue.size() > 0) {
msgStr = msgQueue.front();
reconfigureStreams(msgStr);
//std::cout << "config ID : " << configID << std::endl;
msgQueue.pop_front();
firstConfiguration = false;
}
pthread_mutex_unlock(reconfigMutex);
}
if (config.nodeNum == 1)
checkInterval();
if (streamPixelData(buf) < 0) {
streamerOn = false;
}
doubleBuf->releaseBackBuffer();
//std::cout << "pt1" << std::endl;
}
// for quiting other processes waiting a sync signal
if (config.nodeNum > 1) {
config.syncClientObj->sendSlaveUpdate(frameID);
}
sage::printLog("sageStreamer : network thread exit");
return 0;
}
void PasarMinutoCentinela() {
/* '************************************************* */
/* 'Author: Unknown */
/* 'Last modified: 02/10/2010 */
/* 'Control del timer. Llamado cada un minuto. */
/* '03/10/2010: ZaMa - Adaptado para que funcione mas de un centinela en paralelo. */
/* '************************************************* */
int index;
int UserIndex;
int IdleCount = 0;
if (!centinelaActivado) {
return;
}
/* ' Primero reviso los que estan chequeando usuarios */
for (index = (1); index <= (NRO_CENTINELA); index++) {
/* ' Esta activo? */
if (Centinela[index].Activo) {
Centinela[index].TiempoRestante = Centinela[index].TiempoRestante - 1;
/* ' Temrino el tiempo de chequeo? */
if (Centinela[index].TiempoRestante == 0) {
CentinelaFinalCheck(index);
} else {
UserIndex = Centinela[index].RevisandoUserIndex;
/* 'RECORDamos al user que debe escribir */
if (Distancia(Npclist[Centinela[index].NpcIndex].Pos, UserList[UserIndex].Pos) > 5) {
WarpCentinela(UserIndex, index);
}
/* 'El centinela habla y se manda a consola para que no quepan dudas */
WriteChatOverHead(UserIndex,
"¡" + UserList[UserIndex].Name
+ ", tienes un minuto más para responder! Debes escribir /CENTINELA "
+ vb6::CStr(Centinela[index].clave) + ".",
(Npclist[Centinela[index].NpcIndex].Char.CharIndex), vbRed);
WriteConsoleMsg(UserIndex,
"¡" + UserList[UserIndex].Name + ", tienes un minuto más para responder!",
FontTypeNames_FONTTYPE_CENTINELA);
FlushBuffer(UserIndex);
}
} else {
/* ' Lo reseteo aca, para que pueda hablarle al usuario chequeado aunque haya respondido bien. */
if (Centinela[index].NpcIndex != 0) {
if (Centinela[index].RevisandoUserIndex != 0) {
UserList[Centinela[index].RevisandoUserIndex].flags.CentinelaIndex = 0;
Centinela[index].RevisandoUserIndex = 0;
}
QuitarNPC(Centinela[index].NpcIndex);
Centinela[index].NpcIndex = 0;
}
IdleCount = IdleCount + 1;
}
}
/* 'Verificamos si ya debemos resetear la lista */
int TActual;
TActual = vb6::GetTickCount();
if (checkInterval(centinelaStartTime, TActual, centinelaInterval)) {
/* ' Espero a que terminen de controlar todos los centinelas */
DetenerAsignacion = true;
}
/* ' Si hay algun centinela libre, se fija si no hay trabajadores disponibles para chequear */
if (IdleCount != 0) {
/* ' Si es tiempo de resetear flags, chequeo que no quede nadie activo */
if (DetenerAsignacion) {
/* ' No se completaron los ultimos chequeos */
if (IdleCount < NRO_CENTINELA) {
return;
}
/* ' Resetea todos los flags */
ResetCentinelaInfo();
DetenerAsignacion = false;
/* ' Renuevo el contador de reseteo */
RenovarResetTimer();
}
GoToNextWorkingChar();
}
}
