From ac415ca03cf1a86ae7966b08d7cc13bcb7e9d52e Mon Sep 17 00:00:00 2001 From: Daniel Vayman Date: Tue, 7 May 2024 14:36:55 -0500 Subject: [PATCH] Fixed static occupancy changes --- .../occupancy_cpp/StaticOccupancyNode.hpp | 4 +-- .../occupancy_cpp/src/StaticOccupancyNode.cpp | 30 +++++++++---------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/perception/occupancy_cpp/include/occupancy_cpp/StaticOccupancyNode.hpp b/src/perception/occupancy_cpp/include/occupancy_cpp/StaticOccupancyNode.hpp index 7aa7d6b6..cea876eb 100644 --- a/src/perception/occupancy_cpp/include/occupancy_cpp/StaticOccupancyNode.hpp +++ b/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; diff --git a/src/perception/occupancy_cpp/src/StaticOccupancyNode.cpp b/src/perception/occupancy_cpp/src/StaticOccupancyNode.cpp index b87e8a06..5c2c92b3 100755 --- a/src/perception/occupancy_cpp/src/StaticOccupancyNode.cpp +++ b/src/perception/occupancy_cpp/src/StaticOccupancyNode.cpp @@ -106,8 +106,8 @@ void StaticOccupancyNode::add_points_to_the_DST(pcl::PointCloud // 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);