Skip to content
Snippets Groups Projects
Commit 7c6fea71 authored by Lenny Siol's avatar Lenny Siol
Browse files

start implementing pointcloud to costmap processing

parent 6ce4d123
No related branches found
No related tags found
No related merge requests found
...@@ -13,6 +13,9 @@ find_package(laser_geometry REQUIRED) ...@@ -13,6 +13,9 @@ find_package(laser_geometry REQUIRED)
find_package(tf2_ros REQUIRED) find_package(tf2_ros REQUIRED)
find_package(pcl_ros REQUIRED) find_package(pcl_ros REQUIRED)
find_package(pcl_conversions REQUIRED) find_package(pcl_conversions REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(rptc src/rptc.cpp) add_executable(rptc src/rptc.cpp)
target_include_directories(rptc PUBLIC target_include_directories(rptc PUBLIC
...@@ -20,7 +23,7 @@ target_include_directories(rptc PUBLIC ...@@ -20,7 +23,7 @@ target_include_directories(rptc PUBLIC
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>) $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
target_compile_features(rptc PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_compile_features(rptc PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
ament_target_dependencies(rptc rclcpp sensor_msgs laser_geometry tf2_ros pcl_ros pcl_conversions) ament_target_dependencies(rptc rclcpp sensor_msgs laser_geometry tf2_ros pcl_ros pcl_conversions nav2_costmap_2d nav_msgs std_msgs)
install(TARGETS rptc install(TARGETS rptc
DESTINATION lib/${PROJECT_NAME}) DESTINATION lib/${PROJECT_NAME})
......
...@@ -10,6 +10,9 @@ ...@@ -10,6 +10,9 @@
#include <pcl/features/normal_3d.h> #include <pcl/features/normal_3d.h>
#include <string> #include <string>
#include <vector> #include <vector>
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "std_msgs/msg/header.hpp"
class point_cloud_to_costmap : public rclcpp::Node{ class point_cloud_to_costmap : public rclcpp::Node{
public: public:
...@@ -87,11 +90,56 @@ private: ...@@ -87,11 +90,56 @@ private:
point.x, point.y, point.z, point.normal_x, point.normal_y, point.normal_z); point.x, point.y, point.z, point.normal_x, point.normal_y, point.normal_z);
} }
// Create costmap with 5x5 meters 100x100 cells
nav_msgs::msg::OccupancyGrid costmap = init_costmap(0.05, 100, 100);
// Create 2d grid for gradients
PointNT arr[101][101];
// Align to grid
for (const auto& point : cloud_with_normals->points) {
}
// Sum up
for (const auto& point : cloud_with_normals->points) {
}
// Normalize
for (const auto& point : cloud_with_normals->points) {
}
// Calculate gradients between cells
// Transfer to costmap
// Create grid around robot // Create grid around robot
// Align points to grid
// Average normals in each grid cell
// Calculate gradient between grid cells // set data for costmap: costmap.data[index] = 100
// Return 2D gradient map }
nav_msgs::msg::OccupancyGrid init_costmap(float resolution_in_meters, int width, int height) {
nav_msgs::msg::OccupancyGrid costmap;
// Set the header
costmap.header.stamp = rclcpp::Clock().now();
costmap.header.frame_id = "map";
// Define the costmap properties
costmap.info.resolution = resolution_in_meters; // Resolution in meters per cell
costmap.info.width = width; // Width in cells
costmap.info.height = height; // Height in cells
costmap.info.origin.position.x = 0.0; // Origin of the costmap
costmap.info.origin.position.y = 0.0;
costmap.info.origin.position.z = 0.0;
costmap.info.origin.orientation.w = 1.0;
// Initialize the costmap data with unknown values (-1)
costmap.data.resize(costmap.info.width * costmap.info.height, -1);
return costmap;
} }
}; };
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment