From 50368d84d33fd63833dfa6f57dad8ff6bb1354ff Mon Sep 17 00:00:00 2001
From: dave <david.st-onge@polymtl.ca>
Date: Wed, 20 Dec 2017 13:45:22 -0500
Subject: [PATCH] fix rviz res

---
 buzz_scripts/include/rrtstar.bzz | 2 +-
 src/buzzuav_closures.cpp         | 6 ++++--
 src/roscontroller.cpp            | 9 ++++++---
 3 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz
index 9777dde..2c2a6d5 100644
--- a/buzz_scripts/include/rrtstar.bzz
+++ b/buzz_scripts/include/rrtstar.bzz
@@ -27,7 +27,7 @@ function pathPlanner(m_goal, m_pos, kh4) {
     mapgoal = getcell(math.vec2.add(m_goal, m_pos))
 
     #print_map(map)
-    #export_map(map)
+    export_map(map)
 
     # search for a path
     return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5  * GSCALE.y))  #size(map[1])/20.0,size(map[1])/20.0))
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 980948b..a58c8f2 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -184,12 +184,12 @@ int buzz_exportmap(buzzvm_t vm)
   buzzvm_lload(vm, 1);
   buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);    // dictionary
   buzzobj_t t = buzzvm_stack_at(vm, 1);
-  for(int32_t i = 1; i < buzzdict_size(t->t.value); ++i) {
+  for(int32_t i = 1; i <= buzzdict_size(t->t.value); ++i) {
     buzzvm_dup(vm);
     buzzvm_pushi(vm, i);
     buzzvm_tget(vm);
     std::map<int, int> row;
-    for(int32_t j = 1; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
+    for(int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
       buzzvm_dup(vm);
       buzzvm_pushi(vm, j);
       buzzvm_tget(vm);
@@ -199,6 +199,8 @@ int buzz_exportmap(buzzvm_t vm)
     grid.insert(std::pair<int,std::map<int, int>>(i,row));
     buzzvm_pop(vm);
   }
+  // DEBUG
+  // ROS_INFO("----- Recorded a grid of %i(%i)", grid.size(), buzzdict_size(t->t.value));
   return buzzvm_ret0(vm);
 }
 
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index c5a4354..2bf5cab 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -461,12 +461,14 @@ void roscontroller::grid_publisher()
   int g_h = grid.size();
 
   if(g_w!=0 && g_h!=0) {
+    // DEBUG
+    //ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w);
     auto current_time = ros::Time::now();
     nav_msgs::OccupancyGrid grid_msg;
     grid_msg.header.frame_id = "/world";
     grid_msg.header.stamp = current_time;
     grid_msg.info.map_load_time = current_time;  // Same as header stamp as we do not load the map.
-    //grid_msg.info.resolution = gridMap.getResolution();
+    grid_msg.info.resolution = 0.1;//gridMap.getResolution();
     grid_msg.info.width = g_w;
     grid_msg.info.height = g_h;
     grid_msg.info.origin.position.x = 0.0;
@@ -476,12 +478,13 @@ void roscontroller::grid_publisher()
     grid_msg.info.origin.orientation.y = 0.0;
     grid_msg.info.origin.orientation.z = 0.0;
     grid_msg.info.origin.orientation.w = 1.0;
-    grid_msg.data.resize(g_w*g_h);
+    grid_msg.data.resize(g_w * g_h);
 
     for (itr=grid.begin(); itr!=grid.end(); ++itr) {
       std::map<int,int>::iterator itc = itr->second.begin();
       for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) {
-        grid_msg.data[itr->first*g_w+itc->first] = round(itc->second*100.0);
+        //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, round(itc->second*100.0));
+        grid_msg.data[(itr->first-1)*g_w+itc->first] = round(itc->second*100.0);
       }
     }
     grid_pub.publish(grid_msg);