Merge with Master Branch

mapping/CMakeLists.txt

@@ -0,0 +1,86 @@
+cmake_minimum_required(VERSION 3.5)
+project(mapping)
+
+include(../cmake/default_settings.cmake)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_sensor_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(PCL 1.10 REQUIRED)
+
+include_directories(
+  include
+  ${PCL_INCLUDE_DIRS}
+)
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+# Library creation
+add_library(${PROJECT_NAME} SHARED
+  src/elevation_mapping.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${PCL_LIBRARIES}
+)
+
+set(dependencies
+  rclcpp
+  rclcpp_components
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  tf2
+  tf2_sensor_msgs
+  tf2_geometry_msgs
+  nav_msgs
+  pcl_conversions
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  ${dependencies}
+)
+
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "mapping::ElevationMapping"
+  EXECUTABLE ${PROJECT_NAME}_ElevationMapping
+)
+
+# Install launch files.
+install(
+  DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+# Install library
+install(TARGETS
+  ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

mapping/config/mapping_params.yaml

@@ -0,0 +1,11 @@
+elevation_mapping:
+  ros__parameters:
+    map_frame: "map"
+    camera_frame: "camera_depth_frame"
+    resolution: 0.1
+    width: 60
+    depth_topic: "/depth_camera/points"
+    min_z: 0.1
+    max_z: 1.0
+    inflation_radius: 0.7
+    inflate_obstacles: true

mapping/include/elevation_mapping.hpp

@@ -0,0 +1,63 @@
+#ifndef ELEVATION_MAPPING_HPP_
+#define ELEVATION_MAPPING_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+
+namespace mapping
+{
+
+class ElevationMapping : public rclcpp::Node
+{
+public:
+  explicit ElevationMapping(const rclcpp::NodeOptions & options);
+  ~ElevationMapping();
+
+private:
+  void handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+
+  geometry_msgs::msg::TransformStamped lookup_transform(
+    std::string target_frame,
+    std::string source_frame,
+    rclcpp::Time time);
+
+  bool worldToMap(double x, double y, nav_msgs::msg::MapMetaData info, std::pair<int, int> & out);
+
+  int cellDistance(double world_dist)
+  {
+    return std::ceil(world_dist / resolution_);
+  }
+
+  void inflate(int cell_x, int cell_y, double cell_cost, int radius);
+
+  double gaussian(double x);
+
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr depth_subscriber_;
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_publisher_;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  nav_msgs::msg::OccupancyGrid map_;
+
+  int cell_inflation_radius_;
+  bool inflate_obstacles_;
+
+  int8_t max_cost_ = 100;
+
+  double resolution_;
+  double min_z_;
+  double max_z_;
+  unsigned int width_;
+  std::string map_frame_;
+  std::string camera_frame_;
+};
+
+} // namespace mapping
+
+#endif // ELEVATION_MAPPING_HPP_

mapping/launch/mapping.launch.py

@@ -0,0 +1,27 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    elevation_mapping_node = Node(
+        package="mapping",
+        executable="mapping_ElevationMapping",
+        output="screen",
+        parameters=[
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("mapping"),
+                    "config",
+                    "mapping_params.yaml",
+                ]
+            )
+        ],
+    )
+
+    return LaunchDescription(
+        [
+            elevation_mapping_node,
+        ]
+    )

mapping/package.xml

@@ -0,0 +1,30 @@
+<?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>mapping</name>
+  <version>0.0.0</version>
+  <description>Convert point cloud from depth camera to elevation map</description>
+  <maintainer email="yashas.amb@gmail.com">yambati03</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_sensor_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_1.10</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>