Skip to content
Snippets Groups Projects
Commit 24e7c9e6 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

simplify hello_wolf

parent 2d155506
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #21373 failed
...@@ -247,14 +247,14 @@ int main() ...@@ -247,14 +247,14 @@ int main()
ranges << 1.0; // see drawing ranges << 1.0; // see drawing
bearings << M_PI / 2; bearings << M_PI / 2;
CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb->process(cap_rb); // L1 : (1,2) cap_rb->process(); // L1 : (1,2)
// STEP 2 -------------------------------------------------------------- // STEP 2 --------------------------------------------------------------
t += 1.0; // t : 1.0 t += 1.0; // t : 1.0
// motion // motion
CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov); CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
sensor_odo->process(cap_motion); // KF2 : (1,0,0) cap_motion->process(); // KF2 : (1,0,0)
// observe lmks // observe lmks
ids.resize(2); ids.resize(2);
...@@ -264,14 +264,14 @@ int main() ...@@ -264,14 +264,14 @@ int main()
ranges << sqrt(2.0), 1.0; // see drawing ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3 * M_PI / 4, M_PI / 2; bearings << 3 * M_PI / 4, M_PI / 2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb->process(cap_rb); // L1 : (1,2), L2 : (2,2) cap_rb->process(); // L1 : (1,2), L2 : (2,2)
// STEP 3 -------------------------------------------------------------- // STEP 3 --------------------------------------------------------------
t += 1.0; // t : 2.0 t += 1.0; // t : 2.0
// motion // motion
cap_motion->setTimeStamp(t); cap_motion->setTimeStamp(t);
sensor_odo->process(cap_motion); // KF3 : (2,0,0) cap_motion->process(); // KF3 : (2,0,0)
// observe lmks // observe lmks
ids.resize(2); ids.resize(2);
...@@ -281,7 +281,7 @@ int main() ...@@ -281,7 +281,7 @@ int main()
ranges << sqrt(2.0), 1.0; // see drawing ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3 * M_PI / 4, M_PI / 2; bearings << 3 * M_PI / 4, M_PI / 2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings); cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2) cap_rb->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(1, 0, 1, 0); problem->print(1, 0, 1, 0);
// SOLVE ================================================================ // SOLVE ================================================================
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment