You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
197 lines
7.8 KiB
C++
197 lines
7.8 KiB
C++
#include <iostream>
|
|
#include "Geometry.h"
|
|
#include "Vector3f.h"
|
|
#include "Matrix4D.h"
|
|
#include "FieldNoise.h"
|
|
#include "Line6f.h"
|
|
#include "World.h"
|
|
#include "Field.h"
|
|
#include "LocalizerV2.h"
|
|
|
|
using namespace std;
|
|
|
|
static LocalizerV2& loc = SLocalizerV2::getInstance();
|
|
|
|
void print_python_data(){
|
|
|
|
static World &world = SWorld::getInstance();
|
|
|
|
cout << "Foot touch: " << world.foot_touch[0] << " " << world.foot_touch[1] << endl;
|
|
cout << "LFoot contact rpos: " << world.foot_contact_rel_pos[0].x << " " << world.foot_contact_rel_pos[0].y << " " << world.foot_contact_rel_pos[0].z << endl;
|
|
cout << "RFoot contact rpos: " << world.foot_contact_rel_pos[1].x << " " << world.foot_contact_rel_pos[1].y << " " << world.foot_contact_rel_pos[1].z << endl;
|
|
cout << "Ball seen: " << world.ball_seen << endl;
|
|
cout << "Ball rpos cart: " << world.ball_rel_pos_cart.x << " " << world.ball_rel_pos_cart.y << " " << world.ball_rel_pos_cart.z << endl;
|
|
cout << "Ball cheat: " << world.ball_cheat_abs_cart_pos.x << " " << world.ball_cheat_abs_cart_pos.y << " " << world.ball_cheat_abs_cart_pos.z << endl;
|
|
cout << "Me cheat: " << world.my_cheat_abs_cart_pos.x << " " << world.my_cheat_abs_cart_pos.y << " " << world.my_cheat_abs_cart_pos.z << endl;
|
|
|
|
for(int i=0; i<8; i++){
|
|
cout << "Landmark " << i << ": " <<
|
|
world.landmark[i].seen << " " <<
|
|
world.landmark[i].isCorner << " " <<
|
|
world.landmark[i].pos.x << " " <<
|
|
world.landmark[i].pos.y << " " <<
|
|
world.landmark[i].pos.z << " " <<
|
|
world.landmark[i].rel_pos.x << " " <<
|
|
world.landmark[i].rel_pos.y << " " <<
|
|
world.landmark[i].rel_pos.z << endl;
|
|
}
|
|
|
|
for(int i=0; i<world.lines_polar.size(); i++){
|
|
cout << "Line " << i << ": " <<
|
|
world.lines_polar[i].start.x << " " <<
|
|
world.lines_polar[i].start.y << " " <<
|
|
world.lines_polar[i].start.z << " " <<
|
|
world.lines_polar[i].end.x << " " <<
|
|
world.lines_polar[i].end.y << " " <<
|
|
world.lines_polar[i].end.z << endl;
|
|
}
|
|
}
|
|
|
|
float *compute(bool lfoot_touch, bool rfoot_touch,
|
|
double feet_contact[],
|
|
bool ball_seen, double ball_pos[],
|
|
double me_pos[],
|
|
double landmarks[],
|
|
double lines[],
|
|
int lines_no){
|
|
|
|
// ================================================= 1. Parse data
|
|
|
|
static World &world = SWorld::getInstance();
|
|
world.foot_touch[0] = lfoot_touch;
|
|
world.foot_touch[1] = rfoot_touch;
|
|
|
|
//Structure of feet_contact {lfoot_contact_pt, rfoot_contact_pt, lfoot_contact_rel_pos, rfoot_contact_rel_pos}
|
|
|
|
world.foot_contact_rel_pos[0].x = feet_contact[0];
|
|
world.foot_contact_rel_pos[0].y = feet_contact[1];
|
|
world.foot_contact_rel_pos[0].z = feet_contact[2];
|
|
world.foot_contact_rel_pos[1].x = feet_contact[3];
|
|
world.foot_contact_rel_pos[1].y = feet_contact[4];
|
|
world.foot_contact_rel_pos[1].z = feet_contact[5];
|
|
|
|
world.ball_seen = ball_seen;
|
|
|
|
//Structure of ball_pos {ball_rel_pos_cart, ball_cheat_abs_cart_pos}
|
|
|
|
world.ball_rel_pos_cart.x = ball_pos[0];
|
|
world.ball_rel_pos_cart.y = ball_pos[1];
|
|
world.ball_rel_pos_cart.z = ball_pos[2];
|
|
world.ball_cheat_abs_cart_pos.x = ball_pos[3];
|
|
world.ball_cheat_abs_cart_pos.y = ball_pos[4];
|
|
world.ball_cheat_abs_cart_pos.z = ball_pos[5];
|
|
|
|
world.my_cheat_abs_cart_pos.x = me_pos[0];
|
|
world.my_cheat_abs_cart_pos.y = me_pos[1];
|
|
world.my_cheat_abs_cart_pos.z = me_pos[2];
|
|
|
|
for(int i=0; i<8; i++){
|
|
world.landmark[i].seen = (bool) landmarks[0];
|
|
world.landmark[i].isCorner = (bool) landmarks[1];
|
|
world.landmark[i].pos.x = landmarks[2];
|
|
world.landmark[i].pos.y = landmarks[3];
|
|
world.landmark[i].pos.z = landmarks[4];
|
|
world.landmark[i].rel_pos.x = landmarks[5];
|
|
world.landmark[i].rel_pos.y = landmarks[6];
|
|
world.landmark[i].rel_pos.z = landmarks[7];
|
|
landmarks += 8;
|
|
}
|
|
|
|
|
|
world.lines_polar.clear();
|
|
|
|
for(int i=0; i<lines_no; i++){
|
|
Vector3f s(lines[0],lines[1],lines[2]);
|
|
Vector3f e(lines[3],lines[4],lines[5]);
|
|
world.lines_polar.emplace_back(s, e);
|
|
lines += 6;
|
|
}
|
|
|
|
print_python_data();
|
|
|
|
// ================================================= 2. Compute 6D pose
|
|
|
|
loc.run();
|
|
|
|
// ================================================= 3. Prepare data to return
|
|
|
|
float retval[35];
|
|
float *ptr = retval;
|
|
|
|
for(int i=0; i<16; i++){
|
|
ptr[i] = loc.headTofieldTransform.content[i];
|
|
}
|
|
ptr += 16;
|
|
for(int i=0; i<16; i++){
|
|
ptr[i] = loc.fieldToheadTransform.content[i];
|
|
}
|
|
ptr += 16;
|
|
|
|
ptr[0] = (float) loc.is_uptodate;
|
|
ptr[1] = loc.head_z;
|
|
ptr[2] = (float) loc.is_head_z_uptodate;
|
|
|
|
|
|
return retval;
|
|
}
|
|
|
|
void print_report(){
|
|
loc.print_report();
|
|
}
|
|
|
|
void draw_visible_elements(bool is_right_side){
|
|
Field& fd = SField::getInstance();
|
|
fd.draw_visible(loc.headTofieldTransform, is_right_side);
|
|
}
|
|
|
|
int main(){
|
|
|
|
double feet_contact[] = {0.02668597, 0.055 , -0.49031584, 0.02668597, -0.055 , -0.49031584};
|
|
double ball_pos[] = {22.3917517 , 4.91904904, -0.44419865, -0. , -0. , 0.04 };
|
|
double me_pos[] = {-22.8 , -2.44, 0.48};
|
|
double landmarks[] = { 1. , 1. , -15. , -10. , 0. , 10.88, -37.74, -2.42,
|
|
0. , 1. , -15. , 10. , 0. , 0. , 0. , 0. ,
|
|
1. , 1. , 15. , -10. , 0. , 38.56, -4.9 , -0.66,
|
|
1. , 1. , 15. , 10. , 0. , 39.75, 24.4 , -0.7 ,
|
|
1. , 0. , -15. , -1.05, 0.8 , 7.94, 16.31, 2.42,
|
|
1. , 0. , -15. , 1.05, 0.8 , 8.55, 30.15, 2.11,
|
|
1. , 0. , 15. , -1.05, 0.8 , 37.82, 8.16, 0.5 ,
|
|
1. , 0. , 15. , 1.05, 0.8 , 37.94, 11.77, 0.44 };
|
|
double lines[] = { 25.95, 35.02, -1.14, 24.02, -12.26, -1.12,
|
|
13.18, 59.93, -2.11, 10.87, -37.8 , -2.69,
|
|
39.78, 24.32, -0.75, 38.64, -5.05, -0.67,
|
|
10.89, -37.56, -2.6 , 38.52, -5.24, -0.68,
|
|
15.44, 59.85, -1.87, 39.76, 24.77, -0.88,
|
|
9.62, 3.24, -2.67, 11.02, 36.02, -2.54,
|
|
9.63, 2.82, -3.16, 7.82, 2.14, -3.67,
|
|
11.02, 36.09, -2.61, 9.51, 41.19, -2.94,
|
|
36.03, 5.33, -0.66, 36.46, 14.9 , -0.74,
|
|
35.94, 5.43, -0.72, 37.81, 5.26, -0.73,
|
|
36.42, 14.72, -0.83, 38.16, 14.68, -0.85,
|
|
20.93, 13.26, -1.33, 21.25, 9.66, -1.15,
|
|
21.21, 9.75, -1.6 , 22.18, 7.95, -1.19,
|
|
22.21, 7.94, -1.17, 23.43, 7.82, -1.11,
|
|
23.38, 7.55, -1.18, 24.42, 9.47, -1.16,
|
|
24.43, 9.37, -1.25, 24.9 , 11.72, -0.98,
|
|
24.89, 11.73, -1.2 , 24.68, 14.54, -1.05,
|
|
24.7 , 14.85, -1.06, 23.85, 16.63, -1.1 ,
|
|
23.82, 16.53, -1.14, 22.61, 17.14, -1.32,
|
|
22.65, 17.53, -1.23, 21.5 , 16.19, -1.34,
|
|
21.49, 15.92, -1.32, 20.95, 13.07, -1.32 };
|
|
|
|
int lines_no = sizeof(lines)/sizeof(lines[0])/6;
|
|
|
|
compute(true, // lfoot_touch
|
|
true, // rfoot_touch
|
|
feet_contact,
|
|
true, // ball_seen
|
|
ball_pos,
|
|
me_pos,
|
|
landmarks,
|
|
lines,
|
|
lines_no);
|
|
|
|
}
|
|
|
|
|