commit bccf38e984e2c4f7f3e7bbb1847ae81461451a0e Author: Nils Schulte Date: Sun May 26 16:26:59 2024 +0200 init diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..90151bc --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) +project(simple_slam) + +set(CMAKE_CXX_STANDARD 23) + +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(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED)# uncomment the following section in order to fill in +find_package(nav_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(Ceres REQUIRED) +# further dependencies manually. +# find_package( REQUIRED) + +find_package(Eigen3 REQUIRED NO_MODULE) + +add_executable(simpleslam src/simpleslam_node.cpp ) +ament_target_dependencies(simpleslam rclcpp sensor_msgs nav_msgs) +target_link_libraries(simpleslam Ceres::ceres Eigen3::Eigen) + + +# include_directories(include lib/karto_sdk/include +# ${EIGEN3_INCLUDE_DIRS} +# ${CERES_INCLUDES} +# ) + +install(TARGETS + simpleslam + 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() diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..8734496 --- /dev/null +++ b/package.xml @@ -0,0 +1,34 @@ + + + + simple_slam + 0.0.0 + slam that is hopefully simple + Nils Schulte + AGPL+ + + ament_cmake + + builtin_interfaces + rosidl_default_generators + + eigen + rclcpp + sensor_msgs + nav_msgs + tf2 + tf2_ros + tf2_sensor_msgs + + liblapack-dev + libceres-dev + tbbb + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/simpleslam.hpp b/src/simpleslam.hpp new file mode 100644 index 0000000..cd40520 --- /dev/null +++ b/src/simpleslam.hpp @@ -0,0 +1,32 @@ +#include +#include + +class SimpleSlam { + +public: + struct ScanPoint { + Eigen::Vector2f point; + Eigen::Vector2f normal; + float normal_distance; + }; + void calculate_normals(const std::vector &scan, + std::vector &normals) { + for (unsigned int i = 0; i < scan.size(); i += 1) { + /* TODO comapre not only prev and next but based on distance */ + const auto &prev = scan[(i - 1) % scan.size()]; + const auto &next = scan[(i + 1) % scan.size()]; + ScanPoint p; + float dist_prev = (scan[i] - prev).squaredNorm(); + float dist_next = (scan[i] - next).squaredNorm(); + auto n = dist_next < dist_prev ? (next - scan[i]) : (prev - scan[i]); + const Eigen::Matrix2f r{{0,1},{-1,0}}; + p.normal = r*n; + normals.push_back(p); + + } + }; + void process_scan(const std::vector &scan) { + std::vector points; + calculate_normals(scan, points); + }; +}; diff --git a/src/simpleslam_node.cpp b/src/simpleslam_node.cpp new file mode 100644 index 0000000..418b159 --- /dev/null +++ b/src/simpleslam_node.cpp @@ -0,0 +1,95 @@ +#include + +#include "nav_msgs/msg/map_meta_data.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/srv/get_map.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" + +#include +#include +#include +#include + +#include "simpleslam.hpp" +#include "utils.hpp" + +class MinimalSubscriber : public rclcpp::Node { +public: + SimpleSlam slam; + MinimalSubscriber() : Node("minimal_subscriber") { + pub_map_ = this->create_publisher("/map", 10); + auto topic_callback = + [this](sensor_msgs::msg::LaserScan::UniquePtr msg) -> void { + // 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(), "time_increment'%f'", + // msg->time_increment); + // RCLCPP_INFO(this->get_logger(), "angle_increment '%f'", + // msg->angle_increment); + std::vector scan; + + Eigen::Vector2f min = Eigen::Vector2f(std::numeric_limits::max(), + std::numeric_limits::max()); + Eigen::Vector2f max = Eigen::Vector2f(std::numeric_limits::min(), + std::numeric_limits::min()); + for (size_t i = 0; i < msg->ranges.size(); i += 1) { + auto radius = msg->ranges[i]; + Eigen::Rotation2D rot2(msg->angle_min + + i * msg->angle_increment); + + auto p = rot2 * Eigen::Vector2f(radius, 0); + if (p.array().isInf().any()) + continue; + min = min.cwiseMin(p); + max = max.cwiseMax(p); + scan.push_back(p); + // RCLCPP_INFO(this->get_logger(), "p(%f,%f)",p[0],p[1]); + } + // std::transform(std::views::enumerate(msg->ranges).cbegin(), + // std::views::enumerate(msg->ranges).cend(), scan.begin(), + // [this](const auto &pair) { + // const auto &[index, radius] = pair; + // Eigen::Rotation2D rot2( + // msg->angle_min + index * msg->angle_increment); + // return rot2 * Eigen::Vector2f(radius, 0); + // }); + + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = "map"; + grid.info.resolution = 50.0f / 1000.0f; /* [m/cell] */ + grid.info.width = (max - min)[0] / grid.info.resolution + 1; + grid.info.height = (max - min)[1] / grid.info.resolution + 1; + grid.info.origin.position.x = min[0]; + grid.info.origin.position.y = min[1]; + grid.data.resize(grid.info.width * grid.info.height); + std::fill(grid.data.begin(), grid.data.end(), -1); + auto put_p = [&](const Eigen::Vector2i &pi, const int o) { + int idx = pi.x() + pi.y() * grid.info.width; + // if(idx < grid.data.size()) { + if (grid.data[idx] <= 0) + grid.data[idx] = o; + }; + for (const auto &v : scan) { + Eigen::Vector2i vi = ((v - min) / grid.info.resolution).cast(); + bresenham((-min / grid.info.resolution).cast(), vi, + [&](const auto &p) { put_p(p, 0); }); + put_p(vi, 100); + } + pub_map_->publish(grid); + }; + subscription_ = this->create_subscription( + "scan", 10, topic_callback); + } + +private: + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Publisher::SharedPtr pub_map_; +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/utils.hpp b/src/utils.hpp new file mode 100644 index 0000000..1aaaeeb --- /dev/null +++ b/src/utils.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include +#include // For std::transform +#include // For std::pair +#include +#include +// Helper function to create an enumerator +// auto enumerate(const auto& container) { +// auto indices = std::views::iota(0, static_cast(container.size())); +// return std::views::zip(indices, container); +// } +void bresenham(const Eigen::Vector2i& start, const Eigen::Vector2i& end, std::function f) { + std::vector linePoints; + + int x0 = start.x(), y0 = start.y(); + int x1 = end.x(), y1 = end.y(); + + int dx = std::abs(x1 - x0); + int dy = -std::abs(y1 - y0); + + int sx = (x0 < x1) ? 1 : -1; + int sy = (y0 < y1) ? 1 : -1; + int err = dx + dy; + + while (true) { + f(Eigen::Vector2i(x0, y0)); + if (x0 == x1 && y0 == y1) break; + + int e2 = 2 * err; + if (e2 >= dy) { + err += dy; + x0 += sx; + } + if (e2 <= dx) { + err += dx; + y0 += sy; + } + } + +}