//______________________________________________________________________________
void beamPositionMonitorInterface::updateData( beamPositionMonitorStructs::monitorStruct * ms, const event_handler_args args )
{
/// this could be better, with the type passed from the config
const dbr_time_double * p = ( const struct dbr_time_double * ) args.dbr;
beamPositionMonitorStructs::rawDataStruct * bpmdo = reinterpret_cast< beamPositionMonitorStructs::rawDataStruct *> (ms -> val);
if( bpmdo->isAContinuousMonitorStruct)
{
for( auto && it1 : bpmObj.dataObjects )
{
it1.second.numShots = 1;
it1.second.shotCount = 0;
}
}
const dbr_double_t * value = &(p -> value);
size_t i = 0;
updateTime( p->stamp, bpmdo->timeStamps[ bpmdo->shotCount ], bpmdo->strTimeStamps[ bpmdo->shotCount ] );
for( auto && it : bpmdo->rawBPMData[ bpmdo->shotCount ] )
{
it = *( &p->value + i);
++i;
}
bpmdo->pu1[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 1 ];
bpmdo->pu2[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 2 ];
bpmdo->c1[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 3 ];
bpmdo->p1[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 4 ];
bpmdo->pu3[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 5 ];
bpmdo->pu4[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 6 ];
bpmdo->c2[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 7 ];
bpmdo->p2[ bpmdo->shotCount ] = bpmdo->rawBPMData[ bpmdo->shotCount ][ 8 ];
bpmdo->x[ bpmdo->shotCount ] = calcX( bpmdo->name, bpmdo->pu1[ bpmdo->shotCount ], bpmdo->pu2[ bpmdo->shotCount ], bpmdo->c1[ bpmdo->shotCount ], bpmdo->p1[ bpmdo->shotCount ] );
bpmdo->y[ bpmdo->shotCount ] = calcY( bpmdo->name, bpmdo->pu3[ bpmdo->shotCount ], bpmdo->pu4[ bpmdo->shotCount ], bpmdo->c2[ bpmdo->shotCount ], bpmdo->p2[ bpmdo->shotCount ] );
bpmdo->q[ bpmdo->shotCount ] = calcQ( bpmdo->name, bpmdo->rawBPMData[ bpmdo -> shotCount ] );
if( bpmdo -> isATemporaryMonitorStruct )
{
if( bpmdo -> numShots > -1 )
{
++bpmdo -> shotCount;
}
if( bpmdo->shotCount == bpmdo->numShots )
{
bpmdo->appendingData = false;
// message( "Collected ", bpmdo->shotCount, " shots for ", bpmdo->name );
ms->interface->killCallBack( ms, bpmdo );//, bpmdo );
monitoringData = false;
bpmdo -> shotCount = 0;
}
}
}
FindPI::FindPI(long int epsilon)
{
double pi=0;
#pragma omp parallel reduction(+:pi)
{
#pragma omp for
for (int i = 0; i < epsilon; i++) {
double x = calcX(i, epsilon);
pi += (double)(4 / (1 + x*x));
}
}
printf("MATH.PI=%f",pi/(double)epsilon);
}
/**
applies the system function to x
*/
void GenBaseSystemModel::apply(CVEC &x,
const CVEC &u,
const double deltaT)
{
if((x.GetSize() >= _minXSize) && (u.GetSize() >= _minUSize))
{
//the size is important for the dimension of the Jacobians
_sizeX = x.GetSize();
//do the calculations
calcX(x, u, deltaT);
calcFJacobian(x, deltaT);
calcWJacobian(x, deltaT);
}
}
void CEllipseWindow::OnTimer()
{
RECT rect;
tickCount++;
::GetClientRect(handle, &rect);
if (x == 0 && y == 0) {
x = rect.left + aAxis;
y = rect.top + bAxis;
}
else {
calcX(rect);
calcY(rect);
}
InvalidateRect(handle, &rect, FALSE);
}
void FindPI::CriticalPI(long int epsilon)
{
long int tmp;
int current_thread;
double sum = 0;
#pragma omp parallel
{
double x_i = 0;
#pragma omp for
for (long int i = 0; i < epsilon; i++) {
double x = calcX(i, epsilon);
#pragma omp critical
{
sum += (double)(4 / (1 + x*x));
}
}
}
int zeros = 0;
tmp = epsilon;
while (tmp = tmp / 10) { zeros += 1; }
printf("%4.*f\n", zeros - 1, sum / epsilon);
}
// Update the gait runner and move servos to new positions
// Call it from your main loop or via the scheduler to do it in the background
// NB There is no point scheduling any faster than 20ms as that is the servo refresh rate
// Return true if an animation is playing
boolean gaitRunnerProcess(G8_RUNNER* runner){
if(!gaitRunnerIsPlaying(runner) || runner->speeds==null){
return FALSE;
}
TICK_COUNT now = clockGetus();
int16_t interval = (now - runner->startTime)>>16;
if(interval == 0){
return TRUE;
}
// There has been a noticeable change in time
runner->startTime = now;
if(runner->backwards){
interval *= -1;
}
interval *= runner->speed;
// Re-check as drive speed could be zero
if(interval == 0){
return TRUE;
}
// Locate the current animation
const G8_ANIMATION* animation = &runner->animations[runner->animation];
// Update the current time with the new interval
int16_t currentTime = runner->currentTime + interval;
if(currentTime >= runner->totalTime){
// We have finished playing the animation
if(pgm_read_byte(&animation->sweep)==FALSE){
currentTime %= runner->totalTime; // Set back to start of loop
if(runner->repeatCount){
runner->repeatCount -= 1; // One less frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to final position
}
}
}else{
// Start going backwards through the animation
currentTime = runner->totalTime - (currentTime - runner->totalTime);
runner->backwards = TRUE;
}
}else if(currentTime < 0){
// We have moved before the start
if(pgm_read_byte(&animation->sweep)==FALSE){
currentTime = runner->totalTime + currentTime;
if(runner->repeatCount){
runner->repeatCount += 1; // One more frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to start position
}
}
}else{
// We have completed a sweep
runner->backwards = FALSE;
currentTime = -currentTime;
if(runner->repeatCount){
runner->repeatCount -= 1; // One less frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to initial position
}
}
}
}
runner->currentTime = currentTime; // range is 0....totalTime
// Current time in the range 0...SCALE_X
uint16_t frameTime = interpolateU(currentTime, 0,runner->totalTime, 0, SCALE_X);
uint16_t frameStartTime = 0;
uint16_t frameEndTime = SCALE_X;
// Locate the correct frame
const G8_FRAME* frame = (const G8_FRAME*)pgm_read_word(&animation->frames);
uint8_t i;
for(i = pgm_read_byte(&animation->numFrames)-1; i>0; i--){
const G8_FRAME* f = &frame[i];
frameStartTime = pgm_read_word(&f->time);
if(frameStartTime <= frameTime){
frame = f;
break;
}
frameEndTime = frameStartTime;
frameStartTime = 0;
}
runner->frame = i;
#ifdef DEBUG
rprintf("\n%u,%d",i,currentTime);
#endif
// Now have:- frameStartTime <= frameTime <= frameEndTime
// We now need to find the distance along the curve (0...1) that represents
// the x value = frameTime
// First guess from 0..1
uint16_t frameTimeOffset = frameTime-frameStartTime;
double distanceGuess = ((double)(frameTimeOffset)) / ((double)(frameEndTime-frameStartTime));
const G8_LIMB_POSITION* limb = (const G8_LIMB_POSITION*)pgm_read_word(&frame->limbs);
for(uint16_t l = 0; l < runner->num_actuators; l++, limb++){
double distanceMin = 0.0;
double distanceMax = 1.0;
double distance = distanceGuess;
// Find the correct distance along the line for the required frameTime
for(uint8_t iterations=0; iterations<20; iterations++){
uint16_t actualX = calcX(limb, distance);
if(actualX == frameTimeOffset) break; // Found it
if( actualX < frameTimeOffset){
// We need to increase t
distanceMin = distance;
}else{
distanceMax = distance;
}
// Next guess is half way between
distance = distanceMin + ((distanceMax - distanceMin) / 2);
}
// We now know the distance
runner->speeds[l] = calcY(limb,distance);
#ifdef DEBUG
rprintf(",%d",speed);
#endif
} // next limb
#ifndef DEBUG
// Set all the servo speeds in quick succession
for(uint16_t l = 0; l < runner->num_actuators; l++){
__ACTUATOR* servo = (__ACTUATOR*)pgm_read_word(&runner->actuators[l]);
int16_t speed = (int16_t)(runner->speeds[l]) + (int16_t)(runner->delta[l]);
speed = CLAMP(speed,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX);
__act_setSpeed(servo,(DRIVE_SPEED)speed);
}
#endif
return gaitRunnerIsPlaying(runner);
}
void OpenGLSceneGen::DrawMap()
{
glLoadIdentity();
glTranslatef(0.0f, 0.0f, -1); //restore
float maxX = 0.49f;
float maxY = 0.34f;
float sizeX = .014f;
float sizeY = .022f;
int temp = 0;
int h_MOD = 0;
int W_MOD = 0;
width_scr = 80; //no scrolling
height_scr = 39;
/*int h_MOD=offset.y-height_scr/2; //Y-offset to centre character
if(h_MOD < 0) h_MOD=0 ;
else if(h_MOD > DUNGEON_SIZE_H-height_scr) h_MOD = DUNGEON_SIZE_H-height_scr;
int W_MOD=offset.x-width_scr/2; //X-offset to centre character
if(W_MOD < 0) W_MOD=0;
else if(W_MOD > DUNGEON_SIZE_W-width_scr) W_MOD = DUNGEON_SIZE_W-width_scr;
h_MOD = (h_MOD/5 * 5); //make screen move in steps (this calc works because it is using integers)
W_MOD = (W_MOD/12 * 12);*/
if (dLevel->getMapLight() == DungeonLevel::eNoFound)
addShadows = false;
else
addShadows = true;
freetype::prePrintChar(map_font);
for (int h = 0; h < height_scr; h++)
{
for (int w = 0; w < width_scr; w++)
{
cell & currentCell = dLevel->getCell(w + W_MOD, h + h_MOD);
/*if (World.GetCurrentLevel() == 0) // show all level
;
else*/ if (!showAll && !currentCell.terrain.found) //don't show
continue;
//display extra symbol if lit
if ((currentCell.getSymbol() && currentCell.terrain.light) || (currentCell.getSymbol() && showAll))
{
Symbol* symbol = currentCell.getSymbol();
glColor3ub(symbol->mColour1, symbol->mColour2, symbol->mColour3);
freetype::printChar(map_font, calcX(w), calcY(h), symbol->mSymbol);
}
//display creature if lit
else if ((currentCell.GetMonster() && currentCell.terrain.light) || (currentCell.GetMonster() && showAll))
{
Monster* monster = currentCell.GetMonster();
glColor3ub(monster->color1, monster->color2, monster->color3);
freetype::printChar(map_font, calcX(w), calcY(h), monster->symbol);
if (World.getMonsterManager().FindMonsterData(monster)->GetState() == asleep && World.getMonsterManager().FindMonsterData(monster)->Name() != "rotting ghoul")
freetype::printChar(map_font, calcX(w), calcY(h), '-');
}
//display items
else if (currentCell.getItem() && (addShadows || currentCell.terrain.light))
{
Item * item = currentCell.getItem();
glColor3ub(item->color1, item->color2, item->color3);
// update colour on screen if better than item held
if (!item->hasBrand() && !item->hasResistance())
{
if(World.getMonsterManager().getMonsterItems().isBetter(*World.getMonsterManager().Player(), *item))
glColor3ub(0, 128, 255);
}
freetype::printChar(map_font, calcX(w), calcY(h), currentCell.getItem()->symbol);
}
else if (currentCell.terrain.light) //display terrain
{
glColor3ub((currentCell.terrain.color1), (currentCell.terrain.color2), (currentCell.terrain.color3));
//special water effect
if (currentCell.terrain.type == deepWater && currentCell.terrain.light) //make water flow
{
int flow = World.GetTurns();
if ((h + flow) % 3 == 0)
glColor3ub(0, 64, 128);
else if (((h + flow) + 1) % 3 == 0)
glColor3ub(0, 12, 128);
else
glColor3ub(0, 128, 255);
}
freetype::printChar(map_font, calcX(w), calcY(h), currentCell.terrain.symbol);
//freetype::printChar(map_font, 0 + (w*9.1f), (float)height_scr_offset - (h*14), '.');
}
else if (addShadows)//add night effect
{
glColor3ub((GLubyte)(currentCell.terrain.color1*shadowStrength),
(GLubyte)(currentCell.terrain.color2*shadowStrength),
(GLubyte)(currentCell.terrain.color3*shadowStrength));
freetype::printChar(map_font, calcX(w), calcY(h), currentCell.terrain.symbol);
//freetype::printChar(map_font, 0 + (w*9.1f), (float)height_scr_offset - (h*14.5f), currentCell.terrain.symbol);
}
//add extra characters
if (currentCell.show_target != 0) //show target
{
glColor3ub(0xff, 0xff, 0xff);
freetype::printChar(map_font, calcX(w), calcY(h), 0);
}
if (currentCell.show_path != none) //show target path (overlay)
{
if (currentCell.show_path == clear)
glColor3ub(0xff, 0xff, 0xff);
else if (currentCell.show_path == blocked)
glColor3ub(0xff, 0, 0);
freetype::printChar(map_font, calcX(w), calcY(h) , '*');
}
}
}
freetype::postPrintChar();
}
void update(monitor_odometry_target_data &data, monitor_odometry_target_config config)
{
/* protected region user update on begin */
//Abstand des neue KS vom Roboter (bei Nullgeschwindigkeit).
// config.kindOf_primary_filter = 1;
// config.kindOf_secondary_filter = 5;
// config.kindOf_target_alignment = 0;
// config.noise_threshold = 0.02;
// config.mean_time_threshold = 2;
// config.vel_time_threshold = 2;
// config.vel_threshold = 0.05;
// config.kindOf_stagnancy_override = 0;
// config.mean_vel_threshold = 0.01;
// config.inner_radius = 1.0;
// config.outer_radius = 2.0;
// config.max_vel = 0.30;
// config.min_vel = 0.1;
double output_z = 0;
double radius_of_robot = 1;
ROS_INFO("********************* stage I ***********************");
/*
* stage: (I)
* input odometry data:
* velocity in x;
* velocity in y;
* header:
* timestamp stamp;
* frame_id;
*
*/
x_ = data.in_odometry.twist.twist.linear.x;
y_ = data.in_odometry.twist.twist.linear.y;
header.frame_id = config.base_frame_id;
header.stamp = data.in_odometry.header.stamp;
/*
* info:
*/
//TODO: delete this:
ROS_INFO("input:");
ROS_INFO("x_ = %f",x_);
ROS_INFO("y_ = %f",y_);
//end delete this;
// std::fstream f;
// f.open("testlog_x.dat", std::ios::out | std::ios::app);
// f << x_ << std::endl;
// f.close();
//
// f.open("testlog_y.dat", std::ios::out | std::ios::app);
// f << y_ << std::endl;
// f.close();
//TODO:
ROS_INFO("not good for performace but do anyway!");
velocity_ = calcAbsVec(x_, y_);
//TODO: just for now:
if (velocity_ > config.max_vel) {
ROS_INFO("ERROR vel > max_vel");
}
// velocity_ = calcVel(x_, y_, config.max_vel);
velList = updateVelList(velList, velocity_, header.stamp.toSec(), config.vel_time_threshold);
double meanVel = calcMeanVel(velList);
ROS_INFO("********************* stage II **********************");
/*
* stage: (II)
* primary filters:
*/
if (config.kindOf_primary_filter == 0) {
ROS_INFO("no primary filter active ...");
// velocity_ = calcVel(x_, y_, config.max_vel);
}
else if (config.kindOf_primary_filter == 1) {
ROS_INFO("noise suppression filter active ...");
x_ = noiseSuppression(x_ , config.noise_threshold);
y_ = noiseSuppression(y_ , config.noise_threshold);
// velocity_ = calcVel(x_,y_, config.max_vel);
}
else if (config.kindOf_primary_filter == 2) {
ROS_INFO("velocity based suppression filter active ...");
// velocity_ = calcVel(x_,y_, config.max_vel);
if (velocity_ < config.vel_threshold) {
x_ = 0.0;
y_ = 0.0;
velocity_ = 0.0;
}
}
else if (config.kindOf_primary_filter == 3) {
ROS_INFO("combined suppression filter active ...");
x_ = noiseSuppression(x_ , config.noise_threshold);
y_ = noiseSuppression(y_ , config.noise_threshold);
velocity_ = calcVel(x_,y_, config.max_vel);
if (velocity_ < config.vel_threshold) {
x_ = 0.0;
y_ = 0.0;
velocity_ = 0.0;
//TODO: Ist das nicht gefährlich??? Der Roboter könnte sich bewegen aber die Durchschnittsgeschw.
// über 2-3 Sekunden trotzdem immer null sein???
}
}
else
ROS_WARN("Error - 0001 - primary filter parameter out of bounds!");
ROS_INFO("after primary filters:");
ROS_INFO("x_ = %f",x_);
ROS_INFO("y_ = %f",y_);
//save velocity data
// std::fstream f;
// f.open("testlog_velocity.dat", std::ios::out | std::ios::app);
// f << velocity_ << std::endl;
// f.close();
ROS_INFO("********************* stage III *********************");
/*
* stage: (III)
* target alignment:
*/
//TODO:
if (config.kindOf_target_alignment == 0) {
ROS_INFO("no target alignment active ...");
z_ = (double) hight;
}
else {
//for all cases:
int overrideZ = checkForOverrideZ(config.min_vel, config.max_vel, velocity_);
if (config.kindOf_target_alignment == 1) {
ROS_INFO("vertical target alignment active ...");
z_ = (velocity_ / config.max_vel) * (double) hight;
}
else if (config.kindOf_target_alignment == 2) {
ROS_INFO("linear target alignment active ...");
z_ = (velocity_ / config.max_vel) * (double) hight;
d_ = z_ * ((config.outer_radius - config.inner_radius) / (double) hight);
}
else if (config.kindOf_target_alignment == 3) {
ROS_INFO("elliptical target aligment active ...");
z_ = -1 * sqrt( 1 - (((velocity_ / config.max_vel) / (config.outer_radius - config.inner_radius)) * ((velocity_ / config.max_vel) / (config.outer_radius - config.inner_radius)))) * (double) hight + hight;
d_ = (velocity_ / config.max_vel) * (config.outer_radius - config.inner_radius);
}
else if (config.kindOf_target_alignment == 4) {
ROS_INFO("elliptical target alignment with mean velocity active...");
z_ = -1 * sqrt( 1 - (((meanVel / config.max_vel) / (config.outer_radius - config.inner_radius)) * ((meanVel / config.max_vel) / (config.outer_radius - config.inner_radius)))) * (double) hight + hight;
d_ = (meanVel / config.max_vel) * (config.outer_radius - config.inner_radius);
}
else {
ROS_WARN("Error - 0002 - target alignment parameter out of bounds!");
}
ROS_INFO("OVERRIDE BEFORE IF_ = %u",overrideZ);
if (overrideZ == 0) {
z_ = 0.0;
ROS_INFO("OVI 0");
}
else if (overrideZ == 1) {
z_ = (double) hight;
ROS_INFO("OVI 1=hight");
}
else if (overrideZ == 3) {
ROS_ERROR("Error - 0003 - override parameter out of bounds!");
}
else {
ROS_INFO("OVI wahrscheinlich 2 also nix tun!");
}
}
/*
* stage: (IV)
* secondary filters:
*/
ROS_INFO("********************* stage IV **********************");
if (config.kindOf_secondary_filter == 0) {
ROS_INFO("no secondary filter active ...");
}
else if (config.kindOf_secondary_filter == 1) {
ROS_INFO("mean filter active ...");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> meanXY = calcMeanXY(list_of_values_xystamp_);
x_ = meanXY.at(0);
y_ = meanXY.at(1);
// ROS_INFO("Anzahl Elemente: %lu", list_of_values_xystamp_.size());
// ROS_INFO("meanX %f und meanY %f", meanXY.at(0), meanXY.at(1));
}
else if (config.kindOf_secondary_filter == 2) {
ROS_INFO("median filter active ...");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> medianXY = calcMedianXY(list_of_values_xystamp_);
x_ = medianXY.at(0);
y_ = medianXY.at(1);
}
else if (config.kindOf_secondary_filter == 3) {
ROS_INFO("quantified mean filter slope active ...");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> meanSlope = calcQuantifiedMeanSlope(list_of_values_xystamp_);
x_ = meanSlope.at(0);
y_ = meanSlope.at(1);
}
else if (config.kindOf_secondary_filter == 4) {
ROS_INFO("quantified mean filter stairs active ...");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> meanStairs = calcQuantifiedMeanStairs(list_of_values_xystamp_);
x_ = meanStairs.at(0);
y_ = meanStairs.at(1);
}
else if (config.kindOf_secondary_filter == 5) {
ROS_INFO("quantified mean filter square active");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> meanSquare = calcQuantifiedMeanSquare(list_of_values_xystamp_);
x_ = meanSquare.at(0);
y_ = meanSquare.at(1);
}
else if (config.kindOf_secondary_filter == 6) {
ROS_INFO("quantified median filter slope active");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> medianSlope = calcQuantifiedMedianSlope(list_of_values_xystamp_);
x_ = medianSlope.at(0);
y_ = medianSlope.at(1);
}
else
ROS_WARN("Error - 0003 - secondary filter parameter out of bounds!");
ROS_INFO("after secondary filters:");
ROS_INFO("x_ = %f",x_);
ROS_INFO("y_ = %f",y_);
/*
* stage: (V)
* output production:
*/
// yaw_ = noZeroArcTan(y_,x_);
//for stag test:
//listOfValuesForMeanYaw_ = updateListOfDouble(listOfValuesForMeanYaw_, yaw_, 100);
//end of stag test
// yaw_ = noZeroArcTan(y_,x_);
/*
* stage: (V)
* stagnancy override:
*/
ROS_INFO("********************* stage V ***********************");
if (config.kindOf_stagnancy_override == 0) {
ROS_INFO("no stagnancyoverride active ...");
}
else if (config.kindOf_stagnancy_override == 1) {
ROS_INFO("stagnancy override active ...");
// velList = updateVelList(velList, velocity_, header.stamp.toSec());
// double meanVel = calcMeanVel(velList);
if (meanVel < config.mean_vel_threshold) {
stagnancy_override_counter++;
z_ = (double) hight;
d_ = 0.0;
//wieder zurück drehen???
}
}
else if (config.kindOf_stagnancy_override == 2) {
ROS_INFO("stagnancy override: twist back torso");
if (meanVel < config.mean_vel_threshold) {
// yaw_ = calcMean(listOfValuesForMeanYaw_);
// ROS_INFO("Mean yaw_ %f",yaw_);
}
}
else
ROS_ERROR("Error - 0004 - stagnancy override parameter out of bounds!");
/*
* stage: (VI)
* output calculator:
*/
ROS_INFO("********************* stage VI **********************");
yaw_ = newArcTan(x_,y_);
// ROS_INFO("d_ = %f",d_);
x_ = calcX(yaw_, (config.outer_radius - config.inner_radius) + d_);
y_ = calcY(yaw_, (config.outer_radius - config.inner_radius) + d_);
// x_ = calcX(yaw_, (config.outer_radius - config.inner_radius) + d_);
// y_ = calcY(yaw_, (config.outer_radius - config.inner_radius) + d_);
ROS_INFO("Output: ---------------------");
ROS_INFO("x_ = %f",x_);
ROS_INFO("y_ = %f",y_);
ROS_INFO("yaw_ = %f",yaw_);
ROS_INFO("END ....................");
ROS_INFO("Schwellwert: %f", config.noise_threshold);
if (config.testbool) {
ROS_INFO("testbool is true");
}
else {
ROS_INFO("testbool is false");
}
/*
* stage: (VII)
* tf
*/
ROS_INFO("********************* stage VII *********************");
frame_transform_output(tf::Vector3(x_,y_,z_));
stamped_transform_output.stamp_ = header.stamp;
stamped_transform_output.frame_id_ = config.base_frame_id;
stamped_transform_output.child_frame_id_ = config.lookat_frame_id;
tf::Quaternion q;
q.setRPY(0, 0, yaw_);
stamped_transform_output.setRotation(q);
stamped_transform_output.setOrigin(tf::Vector3(x_,y_,z_));
static tf::TransformBroadcaster br;
br.sendTransform(stamped_transform_output);
// /*
// * evaluations:
// */
// if (active_) {
// tf::Vector3 newOrigin;
// newOrigin.setX(x_);
// newOrigin.setY(y_);
// newOrigin.setZ(z_);
// double distance = calcEuklidianDistance(newOrigin,oldOrigin);
// quickAndDirty++;
// //Problem: Sprung von Null auf den Startwert wieder löschen!
// if (quickAndDirty > 4 ) {
// distances_list.push_back(distance);
// }
//
// double max_value = calcMax(distances_list);
// double sum_value = calcSum(distances_list);
// ROS_INFO("+++++++++++++++++++++++++++++++++++++++");
// ROS_INFO("Auswertung: ");/
// ROS_INFO("Distance = %f",distance);
// ROS_INFO("MAX = %f ", max_value);
// ROS_INFO("SUM = %f", sum_value);
// // showList(distances_list);
// oldOrigin = newOrigin;
//
// //TODO:
// //eval: yaw:
// double yawDistance = calcYawDistance(yaw_, oldYaw);
// if (quickAndDirty > 4) {
// yaw_distances_list.push_back(yawDistance);
// }
// double yaw_max_value = calcMax(yaw_distances_list);
// double yaw_sum_value = calcSum(yaw_distances_list);
// ROS_INFO("++++++++++++++++++++++++++++++++++++++++");
// ROS_INFO("YawDistance = %f", yawDistance);
// ROS_INFO("MAX YAW = %f", yaw_max_value);
// ROS_INFO("SUM YAW = %f ", yaw_sum_value);
// oldYaw = yaw_;
//
// ROS_INFO("stagnacy_override_counter = %u", stagnancy_override_counter);
// }
/* protected region user update end */
}
int main(void)
{
// C MUST BE PRIME to ensure that it is co-prime with other values
const BYTE C = 0x07;
// D MUST be assigned the same value as Y
BYTE D = 0x00;
// Variable for the X, Y, etc. values calculated by the recipient
BYTE calculated_X = 0x00;
BYTE calculated_Y = 0x00;
BYTE calculated_Q = 0x00;
BYTE calculated_R = 0x00;
BYTE calculated_Z = 0x00;
BYTE calculated_Zprime = 0x00;
bool root_found = false;
BYTE Q = 0x00;
BYTE R = 0x00;
BYTE Z1 = 0x00;
BYTE Z2 = 0x00;
BYTE sender_Z1prime = 0x00;
BYTE sender_Z2prime = 0x00;
BYTE recipient_Zprime = 0x00;
// Iterate through every possible combination of the X and Y byte values to verify
// that it is possible to recover X and Y from Z given ANY combinations of values
for (BYTE X = 0x00; X < 0x10; ++X)
{
// For each value of X iterate for each value of Y incremented by 0x01
for (BYTE Y = 0x00; Y < 0x10; ++Y)
{
/**
* Simulate the sender compressing the 2 bytes into a single byte, Z and
* then sending Z to the recipient
*/
D = Y; // D MUST be same value as Y, D is not known to recipient
Q = calcQ(&X, &Y, &C);
R = calcR(&X, &Y, &D);
// Calculate Z using both equivalent methods, Z = R XOR C = Q XOR D
Z1 = calcZ(&R, &C);
Z2 = calcZ(&Q, &D);
// Calcualte Z' using both equivalent methods, Z' = R XNOR C = Q XNOR D
sender_Z1prime = calcZprime(&R, &C);
sender_Z2prime = calcZprime(&Q, &D);
// Verify that Z using both methods is equivalent (This is an AXIOM OF THE
// COMPRESSION ALGORITHM) so if it's wrong we have a major flaw
if (Z1 != Z2)
{
printf("Z1 NOT EQUIVALENT TO Z2!\n");
printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("Z1:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
printf("Z2:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z2)), 0xF0 ^ Z2);
}
// Verify that Z' using both methods is equivalent (This is an AXIOM OF THE
// COMPRESSION ALGORITHM) so if it's wrong we have a major flaw
if (sender_Z1prime != sender_Z2prime)
{
printf("Z1 PRIME NOT EQUIVALENT TO Z2 PRIME!\n");
printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("Z1 PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ sender_Z1prime)), 0xF0 ^ sender_Z1prime);
printf("Z2 PRIME:\t\t"BYTETOBINARYPATTERN", %d\n\n", BYTETOBINARY((0xF0 ^ sender_Z2prime)), 0xF0 ^ sender_Z2prime);
}
/**
* Simulate the recipient receiving a single byte, Z and getting the original
* bytes X and Y from Z, the recipient knows the constant value C, which is prime
*/
// The user can get Z' which is ~Z, this is the UNIQUE property that makes it
// possible to recover X and Y by using the equations of for Z and Z' given the
// two values Z and Z' = ~Z
recipient_Zprime = ~(Z1);
// Verify that the Zprime the recipient gets from ~Z is ACTUALLY, the same Zprime
// that the sender calculated using the equations (This is an AXIOM OF THE
// COMPRESSION ALGORITHM), so if it's wrong we have a major flaw
if (recipient_Zprime != sender_Z1prime)
{
printf("RECIPIENT'S Z PRIME NOT EQUIVALENT TO SENDER'S Z PRIME!!!\n");
printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("Q:\t\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("R:\t\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
}
// Now the user can calculate X very easily given a unique property when C is prime and D == Y
// The user has RECEIVED Z and BOTH recipient and sender KNOW C, UNKNOWN is Y == D
calculated_X = calcX(&Z1, &C);
// Verify that the CALCULATED X IS ACTUALLY EQUAL to the Sender's X value the recipients'
// calculated X should ALWAYS be equivalent to the sender's X when Y == D (This is an AXIOM OF THE
// COMPRESSION ALGORITHM), so if it's wrong we have a major flaw
if (calculated_X != X)
{
printf("RECIPIENT'S CALCULATED X NOT EQUIVALENT TO SENDER'S X!!!\n");
printf("Q:\t\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("R:\t\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("RECIPIENT X:\t"BYTETOBINARYPATTERN"\t\t: %d\n\n", BYTETOBINARY(calculated_X), calculated_X);
}
/**
calculated_Y = 0x00;
calculated_Q = 0x00;
calculated_R = 0x00;
calculated_Z = 0x00;
calculated_Zprime = 0x00;
root_found = false;
calculated_Q = calcQ(&calculated_X, &Y, &C);
calculated_R = calcR(&calculated_X, &Y, &Y);
calculated_Z = calcZ(&calculated_R, &C);
calculated_Zprime = calcZprime(&calculated_Q, &Y);
calculated_Y = calcY(&calculated_Zprime, &calculated_Q);
printf("SENDER Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("CALCULATED Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Q)), 0xF0 ^ calculated_Q);
printf("SENDER R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("CALCULATED R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_R)), 0xF0 ^ calculated_R);
printf("SENDER Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
printf("CALCULATED Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Z)), 0xF0 ^ calculated_Z);
printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
printf("CALCULATED Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(calculated_Zprime), calculated_Zprime);
printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("RECIPIENT X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n\n", BYTETOBINARY(calculated_X),
BYTETOBINARY(calculated_Y), calculated_X, calculated_Y);
*/
// Now solve for the unknown value Y, given the Z' value, X, and C, the only way to feasibly solve
// for Y is to perform a bruteforce search given the known values and the equations for
// Z' and Q. THERE MUST ONLY BE ONE ROOT, Y, GIVEN THE ROOT X , Z' and C (This is an AXIOM OF THE
// COMPRESSION ALGORITHM), so if it's wrong we have a major flaw, in fact its IMPOSSIBLE to
// reverse the compression unless Y == D is UNIQUE!!!!
for (BYTE Y_TEST = 0x00; Y_TEST < 0x10; ++Y_TEST)
{
calculated_Q = calcQ(&calculated_X, &Y_TEST, &C);
calculated_R = calcR(&calculated_X, &Y_TEST, &Y_TEST);
calculated_Z = calcZ(&calculated_R, &C);
calculated_Zprime = calcZprime(&calculated_Q, &Y_TEST);
//printf("Calculated X: %d\n", calculated_X);
//printf("Calculated Y: %d\n", Y_TEST);
//printf("Calculated Q: %d\n", 0xF0 ^ calculated_Q);
//printf("Calculated R: %d\n", 0xF0 ^ calculated_R);
//printf("Calculated Z: %d\n", 0xF0 ^ calculated_Z);
//printf("Calculated Z PRIME: %d\n\n", calculated_Zprime);
/**
printf("SENDER Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("CALCULATED Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Q)), 0xF0 ^ calculated_Q);
printf("SENDER R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("CALCULATED R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_R)), 0xF0 ^ calculated_R);
printf("SENDER Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
printf("CALCULATED Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Z)), 0xF0 ^ calculated_Z);
printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
printf("CALCULATED Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(calculated_Zprime), calculated_Zprime);
printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("RECIPIENT X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n\n", BYTETOBINARY(calculated_X),
BYTETOBINARY(Y_TEST), calculated_X, Y_TEST);
*/
// If the calculated Z' given the calculated root Y matches the known Z' which is
// Z' = ~Z, then the root Y satisfies the equations and IS THE ROOT. Remember that
// for the sender as well as the recipient Y == D, this is the unique property that
// makes it possible to avoid the 0000/1111 cancellations of XOR
if (calculated_X == calcX(&calculated_Z, &C))
{
if (Y_TEST == calcY(&calculated_Zprime, &calculated_Q))
{
if (Z1 == calculated_Z && recipient_Zprime == calculated_Zprime)
{
// X XOR Y SHOULD NOT BE EQUIVALENT TO Z, ONLY THE UNIQUE ROOT IS NOT A COMBINATION
// OF X XOR Y = Z
//if (Y_TEST == Y)
if ((calculated_X ^ Y_TEST) != calculated_Z)
{
calculated_Y = Y_TEST;
root_found = true;
break;
}
}
}
}
}
// If the correct root is found I will be DELIGHTED, the calculated Y MUST
// be UNIQUE and equivalent to the initial Y value of the sender, if it does
// not match we have a major flaw,
if (calculated_Y == Y)
{
printf("RECIPIENT'S CALCULATED Y EQUIVALENT TO SENDER'S Y!!!!!!!!!!!!!!!!!!!!\n");
printf("SENDER Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("CALCULATED Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Q)), 0xF0 ^ calculated_Q);
printf("SENDER R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("CALCULATED R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_R)), 0xF0 ^ calculated_R);
printf("SENDER Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
printf("CALCULATED Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Z)), 0xF0 ^ calculated_Z);
printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
printf("CALCULATED Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(calculated_Zprime), calculated_Zprime);
printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("RECIPIENT X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n", BYTETOBINARY(calculated_X),
BYTETOBINARY(calculated_Y), calculated_X, calculated_Y);
}
// major flaw, back to the drawing board!
else
{
printf("RECIPIENT'S CALCULATED Y NOT EQUIVALENT TO SENDER'S Y!!!!!!!!!!!!!!!!!!!!\n");
printf("SENDER Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("CALCULATED Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Q)), 0xF0 ^ calculated_Q);
printf("SENDER R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("CALCULATED R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_R)), 0xF0 ^ calculated_R);
printf("SENDER Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
printf("CALCULATED Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ calculated_Z)), 0xF0 ^ calculated_Z);
printf("SENDER Z PRIME:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(sender_Z1prime), sender_Z1prime);
printf("RECIPIENT Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(recipient_Zprime), recipient_Zprime);
printf("CALCULATED Z PRIME:\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(calculated_Zprime), calculated_Zprime);
printf("SENDER X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("RECIPIENT X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n", BYTETOBINARY(calculated_X),
BYTETOBINARY(calculated_Y), calculated_X, calculated_Y);
}
}
/**
Zprime = calcZprime(&Q, &D);
calculated_Y = calcY(&Zprime, &Q);
if ( X == calculated_X && Y == calculated_Y)
{
printf("Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Q)), 0xF0 ^ Q);
printf("R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ R)), 0xF0 ^ R);
printf("Z1:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z1)), 0xF0 ^ Z1);
printf("Z2:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY((0xF0 ^ Z2)), 0xF0 ^ Z2);
);
//printf("Z':\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(Zprime), Zprime);
printf("Q:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(Q), Q);
printf("R:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(R), R);
printf("Z:\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(Z), Z);
printf("Z':\t\t"BYTETOBINARYPATTERN", %d\n", BYTETOBINARY(Zprime), Zprime);
printf("INITIAL X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n", BYTETOBINARY(X), BYTETOBINARY(Y), X, Y);
printf("SOLVED X, Y:\t"BYTETOBINARYPATTERN", "BYTETOBINARYPATTERN"\t: %d, %d\n\n",
BYTETOBINARY(calculated_X), BYTETOBINARY(calculated_Y), calculated_X, calculated_Y);
}*/
}
return 0;
}