Add occupancy flattening node (converts to costmap) (#169)

* Updated package file

* Added ROS params

* Updated launch file

* Added package requirements

* Fixed parameter names

* Dimension reduction node

* fix some bugs

* convert to occupancy grid

* fixed segmentation

* cleanup node

* Fix code style issues with Autopep8

* Fix code style issues with clang_format

* Undo change to mpc_test.py

* Undo change to mpc_test.py

---------

Co-authored-by: Sophie Xie <[email protected]>
Co-authored-by: hassaan141 <[email protected]>
Co-authored-by: WATonomousAdmin <[email protected]>

docker/world_modeling/occupancy/occupancy.Dockerfile

@@ -7,7 +7,6 @@ WORKDIR ${AMENT_WS}/src
 
 # Copy in source code 
 COPY src/world_modeling/occupancy occupancy
-COPY src/wato_msgs/sample_msgs sample_msgs
 
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
@@ -21,7 +20,7 @@ FROM ${BASE_IMAGE} as dependencies
 
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
-RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
+RUN apt-get -qq update && apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
 
 # Copy in source code from source stage
 WORKDIR ${AMENT_WS}

src/world_modeling/occupancy/CMakeLists.txt

@@ -1,14 +1,73 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.10)
 project(occupancy)
 
+# Set compiler to use C++ 17 standard
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+# Search for dependencies required for building this package
+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
+
+# Compiles source files into a library
+# A library is not executed, instead other executables can link
+# against it to access defined methods and classes.
+# We build a library so that the methods defined can be used by
+# both the unit test and ROS2 node executables.
+add_library(occupancy_lib
+  src/occupancy_core.cpp)
+# Indicate to compiler where to search for header files
+target_include_directories(occupancy_lib
+  PUBLIC
+  include)
+# Add ROS2 dependencies required by package
+ament_target_dependencies(occupancy_lib
+  sensor_msgs
+  nav_msgs
+  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)
+
+#   # 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()
+
+# Create ROS2 node executable from source files
+add_executable(occupancy_node src/occupancy_node.cpp)
+# Link to the previously built library to access occupancy classes and methods
+target_link_libraries(occupancy_node occupancy_lib)
+
+# Copy executable to installation location
+install(TARGETS
+  occupancy_node
+  DESTINATION lib/${PROJECT_NAME})
+
+# Copy launch and config files to installation location
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME})
 
 ament_package()
+

src/world_modeling/occupancy/config/params.yaml

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

src/world_modeling/occupancy/include/occupancy_core.hpp

@@ -0,0 +1,30 @@
+#ifndef OCCUPANCY_CORE_HPP_
+#define OCCUPANCY_CORE_HPP_
+
+#include <memory>
+#include <vector>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+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.
+   *
+   * @param msg The input PointCloud2 message
+   * @returns the processed point cloud
+   */
+  nav_msgs::msg::OccupancyGrid remove_z_dimension(sensor_msgs::msg::PointCloud2::SharedPtr msg);
+};
+
+#endif  // OCCUPANCY_HPP

src/world_modeling/occupancy/include/occupancy_node.hpp

@@ -0,0 +1,38 @@
+#ifndef OCCUPANCY_NODE_HPP_
+#define OCCUPANCY_NODE_HPP_
+#define PCL_NO_PRECOMPILE
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include "rclcpp/rclcpp.hpp"
+
+#include "occupancy_core.hpp"
+
+/**
+ * Implementation of a ROS2 node that converts 3D PointCloud2 points to 2D by stripping away the
+ * z-dimension.
+ *
+ * Listens to the "nonground_points" topic and removes the z-axis of the 3D points.
+ * Once it processes the points, it publishes it to the "costmap" topic.
+ */
+class OccupancyNode : public rclcpp::Node {
+ public:
+  // Configure pubsub nodes to keep last 20 messages.
+  // https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html
+  static constexpr int ADVERTISING_FREQ = 20;
+
+  /**
+   * Occupancy node constructor.
+   */
+  OccupancyNode();
+
+ private:
+  // Object that handles data processing and validation.
+  OccupancyCore occupancy_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr _subscriber;
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr _publisher;
+
+  void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr point_cloud);
+};
+
+#endif  // OCCUPANCY_NODE_HPP_

src/world_modeling/occupancy/launch/occupancy.launch.py

@@ -0,0 +1,31 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """Launch occupancy node."""
+    occupancy_pkg_prefix = get_package_share_directory('occupancy')
+    occupancy_param_file = os.path.join(
+        occupancy_pkg_prefix, 'config', 'params.yaml')
+
+    occupancy_param = DeclareLaunchArgument(
+        'occupancy_param_file',
+        default_value=occupancy_param_file,
+        description='Path to config file for occupancy node'
+    )
+
+    occupancy_node = Node(
+        package='occupancy',
+        executable='occupancy_node',
+        parameters=[LaunchConfiguration('occupancy_param_file')],
+    )
+
+    return LaunchDescription([
+        occupancy_param,
+        occupancy_node
+    ])

src/world_modeling/occupancy/package.xml

@@ -1,15 +1,21 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>occupancy</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="[email protected]">bolty</maintainer>
-  <license>TODO: License declaration</license>
+  <description>A ROS2 package to turn point cloud into flattened costmap</description>
 
+  <maintainer email="[email protected]">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>
 
+  <test_depend>ament_cmake_gtest</test_depend>
 
+  <!--https://www.ros.org/reps/rep-0149.html#export-->
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/world_modeling/occupancy/src/occupancy_core.cpp

@@ -0,0 +1,98 @@
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+#include <string>
+#include <vector>
+
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include "occupancy_core.hpp"
+
+#include <iostream>
+
+// 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
+  output_costmap.info.resolution = 1.0 / CELLS_PER_METER;  // meters per cell
+
+  // These offsets should always be the same but load them anyway
+  int offset_x = msg->fields[0].offset;
+  int offset_y = msg->fields[1].offset;
+  int offset_z = msg->fields[2].offset;
+  int offset_intensity = msg->fields[3].offset;
+
+  std::vector<int> x_coords;
+  std::vector<int> y_coords;
+  std::vector<float> z_coords;
+
+  int x_low = 0;
+  int y_low = 0;
+  int x_high = 0;
+  int y_high = 0;
+
+  size_t num_cells = 0;
+
+  for (uint i = 0; i < msg->row_step; i += msg->point_step) {
+    float x, y, z, intensity;
+    std::memcpy(&x, &msg->data[i + offset_x], sizeof(float));
+    std::memcpy(&y, &msg->data[i + offset_y], sizeof(float));
+    std::memcpy(&z, &msg->data[i + offset_z], sizeof(float));
+    std::memcpy(&intensity, &msg->data[i + offset_intensity], sizeof(float));
+    // the x and y have to be integers (so round after scaling up by resolution)
+    int x_rounded = std::round(CELLS_PER_METER * x);
+    x_coords.push_back(x_rounded);
+    int y_rounded = std::round(CELLS_PER_METER * y);
+    y_coords.push_back(y_rounded);
+    z_coords.push_back(z);  // TODO: Filter out points that have too high z values
+    // TODO: Transform intensities into 0-100 range integers
+    if (x_rounded >= 0) {
+      x_high = std::max(x_high, x_rounded);
+    } else {
+      x_low = std::min(x_low, x_rounded);
+    }
+    if (y_rounded >= 0) {
+      y_high = std::max(y_high, y_rounded);
+    } else {
+      y_low = std::min(y_low, y_rounded);
+    }
+    num_cells++;
+  }
+
+  // We can now get the height and width of our costmap
+  int width = 1 + x_high - x_low;
+  output_costmap.info.width = width;
+  int height = 1 + y_high - y_low;
+  ;
+  output_costmap.info.height = height;
+  int total_cells = width * height;
+
+  output_costmap.info.origin.position.set__x(-width / (2.0 * CELLS_PER_METER));
+  output_costmap.info.origin.position.set__y(-height / (2.0 * CELLS_PER_METER));
+
+  // Initialize data as all -1 first
+  std::vector<int8_t> data;
+  for (int i = 0; i < total_cells; i++) {
+    data.push_back(-1);
+  }
+
+  // Replace with data from point cloud
+  for (size_t i = 0; i < num_cells; i++) {
+    // 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);
+    data[index] = 0x32;
+  }
+
+  output_costmap.data = std::move(data);
+
+  return output_costmap;
+}

src/world_modeling/occupancy/src/occupancy_node.cpp

@@ -0,0 +1,49 @@
+#include "occupancy_node.hpp"
+
+#include <memory>
+#include <rclcpp/clock.hpp>
+#include <string>
+
+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,
+      std::bind(&OccupancyNode::subscription_callback, this, std::placeholders::_1));
+
+  _publisher = this->create_publisher<nav_msgs::msg::OccupancyGrid>(output_topic, 10);
+}
+
+void OccupancyNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
+  RCLCPP_DEBUG(this->get_logger(), "Received message: %s", msg->header.frame_id.c_str());
+
+  auto start = rclcpp::Clock().now();
+  nav_msgs::msg::OccupancyGrid output_costmap = occupancy_.remove_z_dimension(msg);
+  auto end = rclcpp::Clock().now();
+
+  int duration = (end - start).to_chrono<std::chrono::milliseconds>().count();
+  RCLCPP_DEBUG(this->get_logger(), "Runtime for dimension reduction: %i ms", duration);
+  RCLCPP_DEBUG(this->get_logger(), "3D points: %i", static_cast<int>(msg->width));
+  RCLCPP_DEBUG(this->get_logger(), "Height: %i, Width: %i", output_costmap.info.width,
+               output_costmap.info.height);
+  RCLCPP_DEBUG(this->get_logger(), "Header outgoing: %s", output_costmap.header.frame_id.c_str());
+
+  _publisher->publish(output_costmap);
+}
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<OccupancyNode>());
+  rclcpp::shutdown();
+  return 0;
+}