Fixed static occupancy changes

src/perception/occupancy_cpp/include/occupancy_cpp/StaticOccupancyNode.hpp

@@ -86,12 +86,12 @@ namespace navigator
       const static int event_num = 2;
 
       // Grid size.
-      const static int GRID_SIZE = 60;
+      const static int GRID_SIZE = 128;
 
       const int HALF_SIZE = GRID_SIZE / 2;
 
       // Resolution.
-      constexpr static float res = 1. / 4.;
+      constexpr static float RES = 1. / 3.;
 
       // Measurement mass.
       constexpr static float meas_mass = 0.95;

src/perception/occupancy_cpp/src/StaticOccupancyNode.cpp

@@ -106,8 +106,8 @@ void StaticOccupancyNode::add_points_to_the_DST(pcl::PointCloud<pcl::PointXYZI>
     // Dimensions for X & Y [-64 -> 64 (HALF_SiZE)]
 
     // Record occupancy value for the corresponding point in the pcl, nearest index
-    int x = (int)(cloud[i].x / res);
-    int y = (int)(cloud[i].y / res);
+    int x = (int)(cloud[i].x / RES);
+    int y = (int)(cloud[i].y / RES);
 
     float z = cloud[i].z;
 
@@ -330,7 +330,7 @@ void StaticOccupancyNode::publishOccupancyGrid()
 
   msg.header.stamp = this->clock.clock;
   msg.header.frame_id = "base_link"; // TODO: Make sure the frame is the correct one.
-  msg.info.resolution = res;
+  msg.info.resolution = RES;
   msg.info.width = GRID_SIZE;
   msg.info.height = GRID_SIZE;
   msg.info.origin.position.z = 0.2;
@@ -382,10 +382,10 @@ void StaticOccupancyNode::update_previous()
   // change_x = 0.0; // TODO: Remove or fix the "change" variables.
   // change_y = 0.0;
 
-  // x_new_low = change_x - 64 * res;
-  // x_new_high = change_x + 64 * res;
-  // y_new_low = change_y - 64 * res;
-  // y_new_high = change_y + 64 * res;
+  // x_new_low = change_x - 64 * RES;
+  // x_new_high = change_x + 64 * RES;
+  // y_new_low = change_y - 64 * RES;
+  // y_new_high = change_y + 64 * RES;
 
   // if (initialization_phase == false)
   // {
@@ -417,15 +417,15 @@ void StaticOccupancyNode::update_previous()
   //      * NL = New Low, OH = Old High
   //      */
   //     //x
-  //     int index_xNL = find_nearest(GRID_SIZE, xstart, x_new_low, x_new_high, res);
-  //     int index_xNH = find_nearest(GRID_SIZE, xend, x_new_low, x_new_high, res);
-  //     int index_xOL = find_nearest(GRID_SIZE, xstart, x_old_low, x_old_high, res);
-  //     int index_xOH = find_nearest(GRID_SIZE, xend, x_old_low, x_old_high, res);
+  //     int index_xNL = find_nearest(GRID_SIZE, xstart, x_new_low, x_new_high, RES);
+  //     int index_xNH = find_nearest(GRID_SIZE, xend, x_new_low, x_new_high, RES);
+  //     int index_xOL = find_nearest(GRID_SIZE, xstart, x_old_low, x_old_high, RES);
+  //     int index_xOH = find_nearest(GRID_SIZE, xend, x_old_low, x_old_high, RES);
   //     //y
-  //     int index_yNL = find_nearest(GRID_SIZE, ystart, y_new_low, y_new_high, res);
-  //     int index_yNH = find_nearest(GRID_SIZE, yend, y_new_low, y_new_high, res);
-  //     int index_yOL = find_nearest(GRID_SIZE, ystart, y_old_low, y_old_high, res);
-  //     int index_yOH = find_nearest(GRID_SIZE, yend, y_old_low, y_old_high, res);
+  //     int index_yNL = find_nearest(GRID_SIZE, ystart, y_new_low, y_new_high, RES);
+  //     int index_yNH = find_nearest(GRID_SIZE, yend, y_new_low, y_new_high, RES);
+  //     int index_yOL = find_nearest(GRID_SIZE, ystart, y_old_low, y_old_high, RES);
+  //     int index_yOH = find_nearest(GRID_SIZE, yend, y_old_low, y_old_high, RES);
 
   //     printf("index_xNL: %i, index_xNH: %i, index_xOL: %i, index_xOH: %i\n\n", index_xNL, index_xNH, index_xOL, index_xOH);