fix lidar to cloud conversion

README.md

@@ -10,3 +10,7 @@ gz topic -e -t /lidar
 
 ros2 run ros_gz_bridge parameter_bridge /lidar@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan
 
+source ~/ros2_ws/install/setup.bash
+
+ros2 launch robinspec_pointcloud_to_costmap static_transform_publisher.launch.xml
+ros2 run tf2_tools view_frames
\ No newline at end of file

building_robot.sdf

@@ -58,7 +58,7 @@
                 <visual name='visual'>
                     <geometry>
                         <mesh>
-                            <uri>file:///home/lenny/work/robinspec/test_env.dae</uri>
+                            <uri>test_env.dae</uri>
                         </mesh>
                     </geometry>
                     <material>
@@ -70,7 +70,7 @@
                 <collision name='collision'>
                     <geometry>
                         <mesh>
-                            <uri>file:///home/lenny/work/robinspec/test_env.dae</uri>
+                            <uri>test_env.dae</uri>
                         </mesh>
                     </geometry>
                 </collision>

robinspec_pointcloud_to_costmap/CMakeLists.txt

+cmake_minimum_required(VERSION 3.8)
+project(robinspec_pointcloud_to_costmap)
+
+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)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(laser_geometry REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+
+add_executable(rptc src/rptc.cpp)
+target_include_directories(rptc PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+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)
+
+install(TARGETS rptc
+  DESTINATION lib/${PROJECT_NAME})
+
+# Install launch files
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+# Install package.xml and other necessary files
+install(FILES
+  package.xml
+  DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

robinspec_pointcloud_to_costmap/launch/rptc_simulation.launch.py

+import os
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import ThisLaunchFileDir, PathJoinSubstitution
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # Define the path to the Gazebo ROS package
+    gazebo_ros_pkg_dir = PathJoinSubstitution(
+        ['/opt', 'ros', 'jazzy', 'share', 'ros_gz_sim']
+    )
+
+    # Include the Gazebo launch file
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([gazebo_ros_pkg_dir, '/launch/gz_sim.launch.py']),
+    )
+
+    # Define the path to your custom model
+    model_path = PathJoinSubstitution(
+        [os.path.expanduser('~'), 'path', 'to', 'your', 'model', 'model.sdf']
+    )
+
+    # Spawn the custom model
+    spawn_entity = Node(
+        package='ros_gz_sim',
+        executable='ros_gz_spawn_model.launch.py',
+        arguments=[
+            '-entity', 'my_custom_model',
+            '-file', model_path
+        ],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='true',
+            description='Use simulation (Gazebo) clock if true'
+        ),
+        gazebo,
+        spawn_entity,
+    ])

robinspec_pointcloud_to_costmap/launch/static_transform_publisher.launch.xml

+<!-- static_transform_publisher.launch.xml -->
+<launch>
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="static_transform_publisher" output="screen"
+        args="0 0 0 0 0 0 map base_link" />
+</launch>

robinspec_pointcloud_to_costmap/package.xml

+<?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>robinspec_pointcloud_to_costmap</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="lenny.siol@stud-mail.uni-wuerzburg.de">Lenny Siol</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>laser_geometry</depend>
+  <depend>tf2_ros</depend>
+  <depend>pcl_ros</depend>
+  <depend>pcl_conversions</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
\ No newline at end of file

robinspec_pointcloud_to_costmap/src/rptc.cpp

+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "laser_geometry/laser_geometry.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/features/normal_3d.h>
+#include <string>
+#include <vector>
+
+class point_cloud_to_costmap : public rclcpp::Node{
+public:
+point_cloud_to_costmap() : Node("rptc"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
+  {
+    auto topic_callback =
+      [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) -> void {
+        RCLCPP_INFO(this->get_logger(), "I heard a LaserScan message");
+        // Process the LaserScan message here
+
+	      RCLCPP_INFO(this->get_logger(), "Angle min: %f", msg->angle_min);
+        RCLCPP_INFO(this->get_logger(), "Angle max: %f", msg->angle_max);
+        RCLCPP_INFO(this->get_logger(), "Angle increment: %f", msg->angle_increment);
+        RCLCPP_INFO(this->get_logger(), "Time increment: %f", msg->time_increment);
+        RCLCPP_INFO(this->get_logger(), "Scan time: %f", msg->scan_time);
+        RCLCPP_INFO(this->get_logger(), "Range min: %f", msg->range_min);
+        RCLCPP_INFO(this->get_logger(), "Range max: %f", msg->range_max);
+        RCLCPP_INFO(this->get_logger(), "Ranges size: %zu", msg->ranges.size());
+        // Print out the first few range values as an example
+        for (size_t i = 0; i < std::min(msg->ranges.size(), static_cast<size_t>(5)); ++i) {
+          RCLCPP_INFO(this->get_logger(), "Range[%zu]: %f", i, msg->ranges[i]);
+        }
+
+        process_local_scan(msg);
+      };
+    subscription_ =
+      this->create_subscription<sensor_msgs::msg::LaserScan>("/lidar", 10, topic_callback);
+  }
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
+  laser_geometry::LaserProjection projector_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;  
+
+  typedef pcl::PointXYZ PointT;
+  typedef pcl::PointCloud<PointT> PointCloudT;
+  typedef pcl::PointNormal PointNT;
+  typedef pcl::PointCloud<PointNT> PointCloudNT;
+
+  void process_local_scan(const sensor_msgs::msg::LaserScan::SharedPtr msg){
+    
+    // Turn scan msg into pointcloud msg
+    sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
+    try {
+      projector_.transformLaserScanToPointCloud("vehicle_blue/chassis/gpu_lidar", *msg, *cloud_msg, tf_buffer_);
+      RCLCPP_INFO(this->get_logger(), "Converted LaserScan to PointCloud2");
+    } catch (const std::exception& e) {
+      RCLCPP_ERROR(this->get_logger(), "Error converting LaserScan to PointCloud2: %s", e.what());
+    }
+
+    //TODO
+    //[pcl::fromPCLPointCloud2] No data to copy.
+    //[pcl::NormalEstimation::compute] input_ is empty!
+
+    // Turn pointcloud msg into pcl pointcloud
+    PointCloudT::Ptr cloud(new PointCloudT);
+    PointCloudNT::Ptr cloud_with_normals(new PointCloudNT);
+    pcl::fromROSMsg(*cloud_msg, *cloud);
+
+    pcl::NormalEstimation<PointT, PointNT> ne;
+    ne.setInputCloud(cloud);
+
+    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
+    ne.setSearchMethod(tree);
+
+    ne.setRadiusSearch(0.03);
+
+    ne.compute(*cloud_with_normals);
+
+    for (const auto& point : cloud_with_normals->points) {
+      RCLCPP_INFO(this->get_logger(), "Point: (%.2f, %.2f, %.2f), Normal: (%.2f, %.2f, %.2f)",
+                  point.x, point.y, point.z, point.normal_x, point.normal_y, point.normal_z);
+    }
+
+    // Create grid around robot
+    // Align points to grid
+    // Average normals in each grid cell
+    // Calculate gradient between grid cells
+    // Return 2D gradient map
+  }
+};
+
+int main(int argc, char ** argv)
+{
+  printf("start robinspec pointcloud to costmap\n");
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<point_cloud_to_costmap>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  printf("exit\n");
+  return 0;
+}

robinspec_pointcloud_to_costmap/src/rptc.cpp~

+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+
+class PointCloudToCostmap : public rclcpp::Node{
+public:
+PointCloudToCostmap() : Node("rptc")
+  {
+    auto topic_callback =
+      [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) -> void {
+        RCLCPP_INFO(this->get_logger(), "I heard a LaserScan message");
+        // Process the LaserScan message here
+      };
+    subscription_ =
+      this->create_subscription<sensor_msgs::msg::LaserScan>("/lidar", 10, topic_callback);
+  }
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
+};
+
+int main(int argc, char ** argv)
+{
+  printf("start robinspec pointcloud to costmap\n");
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<PointCloudToCostmap>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  printf("exit\n");
+  return 0;
+}