Skip to content

GitLab

Commit 1bcdfbfb authored by Federico Ciuffardi's avatar Federico Ciuffardi
Browse files

integrated with ros (has bugs)

parent b863ad71
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 1bcdfbfb
......@@ -170,9 +170,9 @@ include_directories(
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/tscf_exploration_node.cpp)
add_executable(${PROJECT_NAME}_map_merger src/Map_Merger/Map_Merger_Node.cpp src/Map_Merger/MapMerger.cpp)
add_executable(${PROJECT_NAME}_central_module src/CentralModule/CentralModule_Node.cpp src/CentralModule/CentralModule.cpp src/lib/GVD/GVD.cpp src/lib/GVD/GVDIC.cpp)
add_executable(${PROJECT_NAME}_central_module src/CentralModule/CentralModule_Node.cpp src/CentralModule/CentralModule.cpp src/lib/GVD/GVD.cpp src/lib/GVD/GVDIC.cpp src/lib/GVD/pos.cpp src/lib/GVD/Map.cpp src/lib/GVD/DisMapIC.cpp)
add_executable(${PROJECT_NAME}_move_controller src/MoveController/move_controller.cpp src/MoveController/TCPClient.cpp)
add_executable(${PROJECT_NAME}_robot src/Robot/RobotNode.cpp src/Robot/Robot.cpp src/lib/GVD/GVD.cpp)
add_executable(${PROJECT_NAME}_robot src/Robot/RobotNode.cpp src/Robot/Robot.cpp src/lib/GVD/GVD.cpp src/lib/GVD/GVDC.cpp src/lib/GVD/pos.cpp)
add_executable(${PROJECT_NAME}_end_module src/end_module.cpp)
#add_executable(${PROJECT_NAME}_monitor src/monitor.cpp src/CentralModule/CentralModule.cpp src/Robot/Robot.cpp)
......
logFiles/office_incrementoGVD2.dat 0 → 100644
View file @ 1bcdfbfb
2.400549 0.133333
3.532236 -0.016667
4.389575 -0.066667
5.555556 0.050000
6.670096 0.016667
7.698903 -0.033334
8.830590 -0.000000
10.065158 0.033334
10.922497 0.000000
12.174212 -0.033333
13.048697 -0.000000
14.300412 0.066667
15.500686 -0.033333
16.803841 0.016667
17.952675 -0.000000
18.878601 0.016667
19.633059 -0.050000
20.456104 0.100000
20.987654 -0.116667
22.016460 0.050000
22.342249 -0.050000
23.473936 0.033333
24.365570 0.083333
25.034294 -0.033333
25.582991 0.066667
26.491770 -0.050000
27.297667 -0.033333
28.086420 0.066666
28.926611 -0.083333
29.869684 0.066666
30.692730 -0.066667
31.721535 0.100000
32.801785 0.066667
33.504803 -0.116667
34.602196 0.000000
35.579563 0.000000
36.711250 0.033333
36.779835 -0.100000
36.779835 -0.050000
36.951302 0.050000
37.431412 0.033333
38.134430 0.116667
38.940327 -0.033333
39.900547 0.033333
40.569271 0.016667
41.203705 -0.050000
41.495197 -0.033333
41.580933 0.033333
41.598080 -0.116667
41.598080 -0.016667
42.095337 0.100000
42.729767 0.016666
43.261318 0.000000
43.964336 -0.016667
44.495884 0.000000
45.233196 0.016667
46.021946 0.066667
46.279152 -0.116667
47.033607 0.050000
47.548012 0.000000
48.165295 -0.033333
48.834019 -0.016667
49.468449 0.133333
49.725651 -0.000000
49.725651 -0.150000
49.965706 0.083333
50.377232 0.033333
50.411522 -0.083333
50.720169 0.050000
51.680386 0.016667
52.794926 0.133333
53.377914 -0.016666
54.183811 -0.100000
54.646778 0.100000
54.783951 -0.100000
54.783951 0.050000
54.938271 0.100000
55.264061 -0.100000
55.264061 -0.150000
55.264061 0.016667
55.264061 0.000000
55.264061 -0.016667
55.264061 0.016667
55.264061 0.016667
55.264061 -0.016667
55.264061 0.016667
55.264061 -0.033334
55.264061 0.016667
55.264061 -0.016666
55.264061 0.000000
55.264061 0.000000
55.264061 0.000000
55.264061 0.033333
55.264061 -0.033334
55.264061 0.016667
55.264061 0.033333
55.264061 -0.016667
55.264061 -0.033333
55.264061 0.000000
55.264061 0.033333
55.264061 -0.033333
55.264061 -0.000000
55.264061 0.016667
55.264061 -0.016667
55.264061 0.000000
55.264061 0.033333
55.264061 -0.033333
55.264061 0.016667
55.264061 -0.016667
55.264061 -0.000000
55.264061 0.000000
55.264061 0.000000
55.264061 -0.000000
55.264061 0.016666
55.264061 -0.000000
55.264061 0.033334
55.264061 -0.050000
55.264061 0.000000
55.264061 0.033333
55.264061 -0.033333
55.264061 0.000000
55.264061 0.016667
55.264061 0.016666
logFiles/office_tiemposGVD2.dat 0 → 100644
View file @ 1bcdfbfb
2.400549 0.133333
3.532236 0.116667
4.389575 0.050000
5.555556 0.100000
6.670096 0.116667
7.698903 0.083333
8.830590 0.083333
10.065158 0.116667
10.922497 0.116667
12.174212 0.083333
13.048697 0.083333
14.300412 0.150000
15.500686 0.116667
16.803841 0.133333
17.952675 0.133333
18.878601 0.150000
19.633059 0.100000
20.456104 0.200000
20.987654 0.083333
22.016460 0.133333
22.342249 0.083333
23.473936 0.116667
24.365570 0.200000
25.034294 0.166667
25.582991 0.233333
26.491770 0.183333
27.297667 0.150000
28.086420 0.216666
28.926611 0.133333
29.869684 0.200000
30.692730 0.133333
31.721535 0.233333
32.801785 0.300000
33.504803 0.183333
34.602196 0.183333
35.579563 0.183333
36.711250 0.216667
36.779835 0.116667
36.779835 0.066667
36.951302 0.116667
37.431412 0.150000
38.134430 0.266667
38.940327 0.233333
39.900547 0.266667
40.569271 0.283333
41.203705 0.233333
41.495197 0.200000
41.580933 0.233333
41.598080 0.116667
41.598080 0.100000
42.095337 0.200000
42.729767 0.216666
43.261318 0.216667
43.964336 0.200000
44.495884 0.200000
45.233196 0.216667
46.021946 0.283333
46.279152 0.166667
47.033607 0.216667
47.548012 0.216667
48.165295 0.183333
48.834019 0.166667
49.468449 0.300000
49.725651 0.300000
49.725651 0.150000
49.965706 0.233333
50.377232 0.266667
50.411522 0.183333
50.720169 0.233333
51.680386 0.250000
52.794926 0.383333
53.377914 0.366667
54.183811 0.266667
54.646778 0.366667
54.783951 0.266667
54.783951 0.316667
54.938271 0.416667
55.264061 0.316667
55.264061 0.166667
55.264061 0.183333
55.264061 0.183333
55.264061 0.166667
55.264061 0.183333
55.264061 0.200000
55.264061 0.183333
55.264061 0.200000
55.264061 0.166667
55.264061 0.183333
55.264061 0.166667
55.264061 0.166667
55.264061 0.166667
55.264061 0.166667
55.264061 0.200000
55.264061 0.166667
55.264061 0.183333
55.264061 0.216667
55.264061 0.200000
55.264061 0.166667
55.264061 0.166667
55.264061 0.200000
55.264061 0.166667
55.264061 0.166667
55.264061 0.183333
55.264061 0.166667
55.264061 0.166667
55.264061 0.200000
55.264061 0.166667
55.264061 0.183333
55.264061 0.166667
55.264061 0.166667
55.264061 0.166667
55.264061 0.166667
55.264061 0.166667
55.264061 0.183333
55.264061 0.183333
55.264061 0.216667
55.264061 0.166667
55.264061 0.166667
55.264061 0.200000
55.264061 0.166667
55.264061 0.166667
55.264061 0.183333
55.264061 0.200000
logFiles1/office_incrementoGVD2.dat 0 → 100644
View file @ 1bcdfbfb
2.400549 0.116667
3.532236 -0.066667
4.406722 0.000000
5.675583 0.016667
6.652949 0.000000
7.956104 0.016667
9.019204 -0.033333
10.185185 0.033334
11.436900 -0.000000
12.414267 0.000000
13.374485 -0.016667
14.626201 0.050000
15.483539 -0.066667
16.135117 0.016667
17.266804 0.033333
17.952675 -0.016666
18.124142 -0.016667
18.295610 0.000000
18.501371 -0.000000
18.501371 -0.033333
19.272978 0.100000
19.907408 -0.033333
20.987654 0.000000
21.604939 -0.000000
22.925240 0.033334
23.645405 0.016666
24.657064 -0.016666
25.308641 0.016666
26.457478 0.016667
26.868999 -0.083333
27.194786 0.050000
27.503431 -0.033334
27.794924 0.000000
28.137859 -0.016667
28.737999 0.050000
29.286694 -0.033334
29.869684 0.000000
30.847052 0.050000
31.893003 0.050000
33.058987 -0.016667
34.207817 0.000000
35.459534 0.050000
36.385460 -0.066667
37.500000 0.100000
38.374485 -0.033333
39.523319 0.116667
40.483540 -0.133333
41.649521 0.016667
42.918381 0.083333
44.461590 0.000000
45.696159 -0.050000
46.793552 0.083333
47.290810 -0.116667
48.045269 0.016667
48.473938 -0.050000
48.765434 0.000000
49.108368 0.016667
49.142662 -0.066667
49.828533 0.116667
50.703018 0.000000
51.491768 0.083333
52.366257 -0.100000
53.206451 0.016667
54.115223 0.150000
54.681068 -0.050000
54.989712 -0.116667
55.486973 0.033333
56.052811 -0.033333
56.464333 0.000000
57.047325 0.033333
58.316189 0.183333
59.379284 -0.116667
60.768173 0.100000
61.728394 -0.016667
logFiles1/office_tiemposGVD2.dat 0 → 100644
View file @ 1bcdfbfb
2.400549 0.116667
3.532236 0.050000
4.406722 0.050000
5.675583 0.066667
6.652949 0.066667
7.956104 0.083333
9.019204 0.050000
10.185185 0.083333
11.436900 0.083333
12.414267 0.083333
13.374485 0.066667
14.626201 0.116667
15.483539 0.050000
16.135117 0.066667
17.266804 0.100000
17.952675 0.083333
18.124142 0.066667
18.295610 0.066667
18.501371 0.066667
18.501371 0.033333
19.272978 0.133333
19.907408 0.100000
20.987654 0.100000
21.604939 0.100000
22.925240 0.133333
23.645405 0.150000
24.657064 0.133333
25.308641 0.150000
26.457478 0.166667
26.868999 0.083333
27.194786 0.133333
27.503431 0.100000
27.794924 0.100000
28.137859 0.083333
28.737999 0.133333
29.286694 0.100000
29.869684 0.100000
30.847052 0.150000
31.893003 0.200000
33.058987 0.183333
34.207817 0.183333
35.459534 0.233333
36.385460 0.166667
37.500000 0.266667
38.374485 0.233333
39.523319 0.350000
40.483540 0.216666
41.649521 0.233333
42.918381 0.316667
44.461590 0.316667
45.696159 0.266667
46.793552 0.350000
47.290810 0.233333
48.045269 0.250000
48.473938 0.200000
48.765434 0.200000
49.108368 0.216667
49.142662 0.150000
49.828533 0.266667
50.703018 0.266667
51.491768 0.350000
52.366257 0.250000
53.206451 0.266667
54.115223 0.416667
54.681068 0.366667
54.989712 0.250000
55.486973 0.283333
56.052811 0.250000
56.464333 0.250000
57.047325 0.283333
58.316189 0.466667
59.379284 0.350000
60.768173 0.450000
61.728394 0.433333
src/CentralModule/CentralModule.cpp
View file @ 1bcdfbfb
......@@ -33,10 +33,10 @@ void CentralModule::setCentrosF(vector<int> newCentrosF) {
};
/* De ocupancy grid a state grid
grid_type og2gt(nav_msgs::OccupancyGrid og, vector<int> frontera) {
grid og2gt(nav_msgs::OccupancyGrid og, vector<int> frontera) {
uint mapWidth = og.info.width;
uint mapHeight = og.info.height;
grid_type res;
grid res;
for (int x = 0; x < mapWidth; x++) {
res.push_back(row_type());
for (int y = 0; y < mapHeight; y++) {
......@@ -87,13 +87,13 @@ tscf_exploration::SegmentAuction CentralModule::getSegmentAuctionInfo() {
cout << "debug :: apply kmeans" << endl;
aplicarKmeans(frontera);
grid_type gt = og2gt(map, getCentrosF(), &cell_count);
grid gt = og2gt(map, getCentrosF(), &cell_count);
// criticals_info cis_aux;
cout << "debug :: gvd and cis" << endl;
GVD gvd;
//boost::tie(cis, gvd) = get_points_of_interest(gt);
boost::tie(cis, gvd) = igvdc.get_points_of_interest(gt);
boost::tie(cis, gvd) = gvdic.get_points_of_interest(gt);
cout << "debug :: gvd to rosmsg" << endl;
GVD::VertexIterator v_it, v_it_end;
......@@ -624,7 +624,7 @@ void CentralModule::setObstaculos(vector<int> newObstaculos) {
//taking into account the vision range of robot and leaving only significants frontiers
CentralModule::aplicarKmeans(CentralModule::frontera);
grid_type gt = og2gt(ret.mapa, CentralModule::getCentrosF());
grid gt = og2gt(ret.mapa, CentralModule::getCentrosF());
criticals_info cis;
GVD gvd;
......
src/CentralModule/CentralModule.h
View file @ 1bcdfbfb
......@@ -36,7 +36,7 @@ class CentralModule {
// vars
/// GVD
GVDIC igvdc;
GVDIC gvdic;
/// state
centralMouleState estado;
int segment_auction_id;
......@@ -50,7 +50,7 @@ class CentralModule {
/// Map related
nav_msgs::OccupancyGrid map_merged;
boost::unordered_map<int, cv::Point2f> map_points;
grid_type last_map;
grid last_map;
/// frontier related
boost::unordered_set<int> frontera;
......
src/CentralModule/CentralModule_Node.cpp
View file @ 1bcdfbfb
......@@ -25,7 +25,7 @@ ros::Publisher segment_auction_pub;
map<string, ros::Publisher> segment_assignment_pubs;
// others
ros::Timer auctionResolutionTimer;
ros::Duration AuctionResolutionTimeout(0.1); // first AuctionResolutionTimeout
ros::Duration AuctionResolutionTimeout(1); // first AuctionResolutionTimeout
int succesfulBids = 0;
ros::Timer auctionStartDelayTimer;
......@@ -37,7 +37,7 @@ ros::Duration auctionStartTimeout; //(5.0);
int assigned_robots = 0;
int requests = 0;
CentralModule centralModule;
CentralModule *centralModule;
bool FIN = false;
ros::Time last_auction_start;
ros::Time last_gvd_start;
......@@ -97,14 +97,14 @@ static void draw_gvd(tscf_exploration::SegmentAuction sac, map_info_type map_inf
void startAuction() {
// initialization
map_info_type map_info = centralModule.getMap().info;
map_info_type map_info = centralModule->getMap().info;
// get aution info
tscf_exploration::SegmentAuction segment_auction;
last_gvd_start = ros::Time::now();
ROS_INFO("Computing the segment auction to publish");
segment_auction = centralModule.getSegmentAuctionInfo();
segment_auction = centralModule->getSegmentAuctionInfo();
ROS_INFO("Computed the segment auction to publish");
cout << "debug :: getSegmentAuctionInfo done" << endl;
ros::Duration last_gvd_time = gvd_time;
......@@ -117,19 +117,19 @@ void startAuction() {
ros::Duration current_time = ros::Time::now() - first_auction;
// string data = to_string(current_time.toSec())+" "+to_string(gvd_time.toSec());
string time = to_string(current_time.toSec());
string coverage_per = to_string(((float)centralModule.cell_count / MAP_SIZE) * 100);
string coverage_per = to_string(((float)centralModule->cell_count / MAP_SIZE) * 100);
if (coverage_file_log.empty()) {
coverage_file_log = "covarage";
coverage_file_log = LOG_FILE_PATH + map_name + "_" + coverage_file_log +
to_string(centralModule.getNumRobots());
to_string(centralModule->getNumRobots());
}
string data = coverage_per + " " + to_string(gvd_time.toSec());
if (gvd_file_log.empty()) {
gvd_file_log = "tiemposGVD";
gvd_file_log =
LOG_FILE_PATH + map_name + "_" + gvd_file_log + to_string(centralModule.getNumRobots());
LOG_FILE_PATH + map_name + "_" + gvd_file_log + to_string(centralModule->getNumRobots());
}
log_data(data, gvd_file_log);
......@@ -140,7 +140,7 @@ void startAuction() {
if (increment_gvd_file_log.empty()) {
increment_gvd_file_log = "incrementoGVD";
increment_gvd_file_log = LOG_FILE_PATH + map_name + "_" + increment_gvd_file_log +
to_string(centralModule.getNumRobots());
to_string(centralModule->getNumRobots());
}
log_data(data, increment_gvd_file_log);
......@@ -155,7 +155,7 @@ void startAuction() {
// Send auction info: Graph and criticals info
requests = 0;
centralModule.setEstado(WaitingFirstBid);
centralModule->setEstado(WaitingFirstBid);
segment_auction_pub.publish(segment_auction);
ROS_INFO("Segment Auction published");
......@@ -163,7 +163,7 @@ void startAuction() {
void resolveAuction() {
cout << "debug :: resolveAuction start" << endl;
centralModule.setEstado(Resolving);
centralModule->setEstado(Resolving);
ros::Duration resolution_time = ros::Time::now() - last_auction_start;
ROS_INFO("Auction Resolution Start, expected %f, real %f", AuctionResolutionTimeout.toSec(),
resolution_time.toSec());
......@@ -175,17 +175,17 @@ void resolveAuction() {
// set up
visualization_msgs::Marker::_points_type points;
map_info_type map_info = centralModule.getMap().info;
map_info_type map_info = centralModule->getMap().info;
cout << "debug :: resolveAuction assignSegment" << endl;
boost::unordered_map<string, tscf_exploration::SegmentAssignment> assignment =
centralModule.assignSegment();
centralModule->assignSegment();
assigned_robots = assignment.size();
float max_estimated_time = 0;
float min_estimated_time = FLT_MAX;
centralModule.setEstado(WaitingAuction);
centralModule->setEstado(WaitingAuction);
cout << "debug :: resolveAuction timer stuff" << endl;
for (auto it = assignment.begin(); it != assignment.end(); it++) {
......@@ -197,7 +197,7 @@ void resolveAuction() {
// obtain costs
float estimated_time =
(centralModule.segment_bids[robot][p2d_to_pos(sa.segment)]) / (ROBOT_SPEED);
(centralModule->segment_bids[robot][p2d_to_pos(sa.segment)]) / (ROBOT_SPEED);
max_estimated_time = max(max_estimated_time, estimated_time);
min_estimated_time = max(min_estimated_time, estimated_time);
......@@ -211,7 +211,7 @@ void resolveAuction() {
tscf_exploration::SegmentAssignment sa = it->second;
string robot = it->first;
float estimated_time =
(centralModule.segment_bids[robot][p2d_to_pos(sa.segment)]) / (ROBOT_SPEED);
(centralModule->segment_bids[robot][p2d_to_pos(sa.segment)]) / (ROBOT_SPEED);
if (estimated_time >
min_estimated_time + (gvd_time + gvd_time_increment + auctionStartDelayTimeout).toSec()) {
assigned_robots--;
......@@ -245,7 +245,7 @@ void resolveAuction() {
void auctionResolutionTimerRoutine(const ros::TimerEvent&) {
auctionResolutionTimer.stop();
if (centralModule.getEstado() == WaitingBids) {
if (centralModule->getEstado() == WaitingBids) {
resolveAuction();
} else {
ROS_INFO("WARNING: auction AuctionResolutionTimeout with no bids");
......@@ -271,7 +271,7 @@ void auctionStartDelayTimerRoutine(const ros::TimerEvent&) {
/* cuando: un robot te pide un objetivo */
/* que: inicia una subasta */
void handleRequest(const std_msgs::StringConstPtr& msg) {
if (centralModule.getEstado() != WaitingAuction) {
if (centralModule->getEstado() != WaitingAuction) {
ROS_INFO("Auction request from ignored, already one on course");
} else {
requests++;
......@@ -294,7 +294,7 @@ bool first = true;
/* que: actualizar mapa si no se estan esperando ofertas ni se termino
y si es el decimo mapa recibido inicia una subasta*/
void handleNewMap(const tscf_exploration::mapMergedInfoConstPtr& msg) {
centralModule.updateMap(msg);
centralModule->updateMap(msg);
if (first) {
first_auction = ros::Time::now();
first = false;
......@@ -309,7 +309,7 @@ void handleEnd(const std_msgs::StringConstPtr& msg) {
// log data
if (LOG > 0) {
ros::Duration current_time = ros::Time::now() - first_auction;
string robot_count = to_string(centralModule.getNumRobots());
string robot_count = to_string(centralModule->getNumRobots());
string time = to_string(current_time.toSec());
string data = "Cantidad Robots: " + robot_count + "\n";
......@@ -341,12 +341,12 @@ void handleEnd(const std_msgs::StringConstPtr& msg) {
}
void handleSegmentBid(const tscf_exploration::SegmentBidConstPtr& msg, string name) {
// ROS_INFO("Got segment_bid %d",centralModule.getEstado());
if (centralModule.getEstado() != WaitingFirstBid && centralModule.getEstado() != WaitingBids) {
// ROS_INFO("Got segment_bid %d",centralModule->getEstado());
if (centralModule->getEstado() != WaitingFirstBid && centralModule->getEstado() != WaitingBids) {
return;
}
bool successful = centralModule.saveSegmentBid(*msg, name);
bool successful = centralModule->saveSegmentBid(*msg, name);