cleanup node

modules/docker-compose.world_modeling.yaml

@@ -32,7 +32,6 @@ services:
         - "${WORLD_MODELING_OCCUPANCY_IMAGE}:build_main"
       target: deploy
     image: "${WORLD_MODELING_OCCUPANCY_IMAGE}:${TAG}"
-    restart: always # this is just temporarily while we figure why it's seg faulting sometimes
     command: /bin/bash -c "ros2 launch occupancy occupancy.launch.py"
 
   occupancy_segmentation:

src/world_modeling/occupancy/CMakeLists.txt

@@ -15,7 +15,6 @@ find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
-find_package(pcl_ros REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -32,28 +31,27 @@ target_include_directories(occupancy_lib
 ament_target_dependencies(occupancy_lib
   sensor_msgs
   nav_msgs
-  rclcpp
-  pcl_ros)
+  rclcpp)
 
-# By default tests are built. This can be overridden by modifying the
-# colcon build command to include the -DBUILD_TESTING=OFF flag.
-option(BUILD_TESTING "Build tests" ON)
-if(BUILD_TESTING)
-  # Search for dependencies required for building tests + linting
-  find_package(ament_cmake_gtest REQUIRED)
+# # By default tests are built. This can be overridden by modifying the
+# # colcon build command to include the -DBUILD_TESTING=OFF flag.
+# option(BUILD_TESTING "Build tests" ON)
+# if(BUILD_TESTING)
+#   # Search for dependencies required for building tests + linting
+#   find_package(ament_cmake_gtest REQUIRED)
 
-  # Create test executable from source files
-  ament_add_gtest(occupancy_test test/occupancy_test.cpp)
-  # Link to the previously built library to access Transformer classes and methods
-  target_link_libraries(occupancy_test
-    occupancy_lib
-    gtest_main)
+#   # Create test executable from source files
+#   ament_add_gtest(occupancy_test test/occupancy_test.cpp)
+#   # Link to the previously built library to access Transformer classes and methods
+#   target_link_libraries(occupancy_test
+#     occupancy_lib
+#     gtest_main)
 
-  # Copy executable to installation location
-  install(TARGETS
-    occupancy_test
-    DESTINATION lib/${PROJECT_NAME})
-endif()
+#   # Copy executable to installation location
+#   install(TARGETS
+#     occupancy_test
+#     DESTINATION lib/${PROJECT_NAME})
+# endif()
 
 # Create ROS2 node executable from source files
 add_executable(occupancy_node src/occupancy_node.cpp)

src/world_modeling/occupancy/config/params.yaml

@@ -1,7 +1,7 @@
 occupancy_node:
   ros__parameters:
     version: 1
-    compression_method: 0
     subscription_topic: /nonground_points
     publish_topic: /costmap
+    resolution: 3 # resolution in cells/m
 

src/world_modeling/occupancy/include/occupancy_core.hpp

@@ -10,10 +10,14 @@
 
 class OccupancyCore {
  public:
+  // Resolution of the costmap (in cells/m)
+  int CELLS_PER_METER;
+
   /**
    * OccupancyCore constructor.
    */
   OccupancyCore();
+  OccupancyCore(int resolution);
 
   /**
    * Removes the z-axis dimension from the given PointCloud2 message.

src/world_modeling/occupancy/package.xml

@@ -2,17 +2,16 @@
 <package format="3">
   <name>occupancy</name>
   <version>0.0.0</version>
-  <description>A ROS2 package remove the third dimension of points</description>
+  <description>A ROS2 package to turn point cloud into flattened costmap</description>
 
-  <maintainer email="syxie@watonomous.ca">Sophie Xie</maintainer>
+  <maintainer email="v3jayaku@watonomous.ca">Vishal Jayakumar</maintainer>
   <license>NA</license>
 
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
-  <depend>pcl_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
 

src/world_modeling/occupancy/src/occupancy_core.cpp

@@ -11,16 +11,15 @@
 
 // Constructor
 OccupancyCore::OccupancyCore() {}
+OccupancyCore::OccupancyCore(int resolution) : CELLS_PER_METER{resolution} {}
 
 nav_msgs::msg::OccupancyGrid OccupancyCore::remove_z_dimension(sensor_msgs::msg::PointCloud2::SharedPtr msg) {
     nav_msgs::msg::OccupancyGrid output_costmap;
 
     // Initialize the output point cloud
     output_costmap.header = msg->header;
     output_costmap.info.map_load_time = msg->header.stamp; // Not completely sure about what timestamp to use here
-    int CELLS_PER_METER = 3;
     output_costmap.info.resolution = 1.0 / CELLS_PER_METER; // meters per cell
-    // TODO: Read the denominator in the above line from parameters
 
     // These offsets should always be the same but load them anyway
     int offset_x = msg->fields[0].offset;
@@ -88,7 +87,7 @@ nav_msgs::msg::OccupancyGrid OccupancyCore::remove_z_dimension(sensor_msgs::msg:
       // Shift x and y so the center is at width/2 and height/2
       int shifted_x = x_coords[i] + width/2;
       int shifted_y = y_coords[i] + height/2;
-      int index = std::min(total_cells - 1, shifted_y*width + shifted_x); // This is a stopgap, have to figure out why it's exceeding total_cells
+      int index = std::min(total_cells - 1, shifted_y*width + shifted_x);
       data[index] = 0x32; 
     }
 

src/world_modeling/occupancy/src/occupancy_node.cpp

@@ -9,10 +9,14 @@ OccupancyNode::OccupancyNode() : Node("occupancy") {
   // Declare ROS parameters
   this->declare_parameter<std::string>("subscription_topic", std::string("/nonground_points"));
   this->declare_parameter<std::string>("publish_topic", std::string("/costmap"));
+  this->declare_parameter<int>("resolution", 3);
 
   // Fetch parameters
   auto input_topic = this->get_parameter("subscription_topic").as_string();
   auto output_topic = this->get_parameter("publish_topic").as_string();
+  auto resolution = this->get_parameter("resolution").as_int();
+
+  occupancy_ = OccupancyCore(resolution);
 
   _subscriber = this->create_subscription<sensor_msgs::msg::PointCloud2>(
       input_topic, ADVERTISING_FREQ,