-
Notifications
You must be signed in to change notification settings - Fork 4
/
test_fastsim.cpp
106 lines (93 loc) · 3.18 KB
/
test_fastsim.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
#ifndef __APPLE__
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE fastsim
#include <boost/test/unit_test.hpp>
#endif
#include <boost/foreach.hpp>
#include <sferes/fit/fitness_simu.hpp>
#include "simu_fastsim.hpp"
using namespace sferes;
//using namespace sferes::ctrl;
struct Params {
struct simu {
SFERES_STRING(map_name, "modules/fastsim/cuisine.pbm");
SFERES_CONST float dt = 0.01;
};
};
#ifndef __APPLE__
BOOST_AUTO_TEST_CASE(fastsim_intersection) {
using namespace fastsim;
typedef simu::Fastsim<Params> simu_t;
simu_t s;
s.robot().set_pos(Posture(200, 100, 0));
s.robot().use_camera();
Map::ill_sw_t s1 = Map::ill_sw_t(new IlluminatedSwitch(1, 10, 300, 250, true));
Map::ill_sw_t s2 = Map::ill_sw_t(new IlluminatedSwitch(2, 30, 500, 250, true));
Map::ill_sw_t s3 = Map::ill_sw_t(new IlluminatedSwitch(3, 10, 200, 150, true));
Map::ill_sw_t s4 = Map::ill_sw_t(new IlluminatedSwitch(4, 10, 400, 350, true));
s.map()->add_illuminated_switch(s1);
s.map()->add_illuminated_switch(s2);
s.map()->add_illuminated_switch(s3);
s.map()->add_illuminated_switch(s4);
int c1 = s.map()->check_inter_is(250, 250, 1000, 250);
BOOST_CHECK_EQUAL(c1, 1);
int c2 = s.map()->check_inter_is(250, 250, 250, 0);
BOOST_CHECK_EQUAL(c2, -1);
int c3 = s.map()->check_inter_is(200, 350, 200, 100);
BOOST_CHECK_EQUAL(c3, 3);
// s.init_view();
// while(true)
// {
// s.refresh();
// s.refresh_view();
// }
}
#endif
#ifdef __APPLE__
int main(int argc, char *argv[]) {
std::cout<<"WARNING: BOOST TEST framework desactivated on macos to avoid problems with SDL."<<std::endl;
#else
BOOST_AUTO_TEST_CASE(fastsim_simu_refresh) {
#endif
using namespace fastsim;
typedef simu::Fastsim<Params> simu_t;
simu_t s;
s.robot().set_pos(Posture(250, 250, 0));
s.robot().add_laser(Laser(M_PI / 3, 50));
s.robot().add_laser(Laser(0, 50));
s.robot().add_laser(Laser(-M_PI / 3, 50));
Map::ill_sw_t s1 = Map::ill_sw_t(new IlluminatedSwitch(1, 10, 300, 251, true));
Map::ill_sw_t s2 = Map::ill_sw_t(new IlluminatedSwitch(2, 10, 530, 150, false));
Map::ill_sw_t s3 = Map::ill_sw_t(new IlluminatedSwitch(3, 10, 200, 150, false));
Map::ill_sw_t s4 = Map::ill_sw_t(new IlluminatedSwitch(4, 10, 400, 350, false));
s1->link(s2);
s1->link(s3);
s2->link(s4);
s.map()->add_illuminated_switch(s1);
s.map()->add_illuminated_switch(s2);
s.map()->add_illuminated_switch(s3);
s.map()->add_illuminated_switch(s4);
s.robot().add_light_sensor(LightSensor(1, M_PI / 3, M_PI / 3));
s.robot().add_light_sensor(LightSensor(1, -M_PI / 3, M_PI / 3));
s.robot().add_light_sensor(LightSensor(2, M_PI / 3, M_PI / 3));
s.robot().add_light_sensor(LightSensor(2, -M_PI / 3, M_PI / 3));
s.init_view();
for (size_t i = 0; i < 10000; ++i) {
if (s.robot().get_left_bumper())
s.move_robot(2.0, -2.0);
else if (s.robot().get_right_bumper())
s.move_robot(2.0, -2.0);
else if (rand()/(RAND_MAX + 1.0) < 0.05)
s.move_robot(3.0, -3.0);
else if (rand()/(RAND_MAX + 1.0) < 0.05)
s.move_robot(-3.0, 3.0);
else
s.move_robot(2.0, 2.0);
std::cout<<"i="<<i<<std::endl;
s.refresh();
s.refresh_view();
}
#ifdef __APPLE__
return 0;
#endif
}