init
This commit is contained in:
commit
bccf38e984
57
CMakeLists.txt
Normal file
57
CMakeLists.txt
Normal file
@ -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(<dependency> 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()
|
||||
34
package.xml
Normal file
34
package.xml
Normal file
@ -0,0 +1,34 @@
|
||||
<?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>simple_slam</name>
|
||||
<version>0.0.0</version>
|
||||
<description>slam that is hopefully simple</description>
|
||||
<maintainer email="git@nilsschulte.de">Nils Schulte</maintainer>
|
||||
<license>AGPL+</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<build_depend>builtin_interfaces</build_depend>
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
|
||||
<depend>eigen</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_sensor_msgs</depend>
|
||||
|
||||
<build_depend>liblapack-dev</build_depend>
|
||||
<build_depend>libceres-dev</build_depend>
|
||||
<build_depend>tbb</build_depend>b
|
||||
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
32
src/simpleslam.hpp
Normal file
32
src/simpleslam.hpp
Normal file
@ -0,0 +1,32 @@
|
||||
#include <Eigen/Dense>
|
||||
#include <vector>
|
||||
|
||||
class SimpleSlam {
|
||||
|
||||
public:
|
||||
struct ScanPoint {
|
||||
Eigen::Vector2f point;
|
||||
Eigen::Vector2f normal;
|
||||
float normal_distance;
|
||||
};
|
||||
void calculate_normals(const std::vector<Eigen::Vector2f> &scan,
|
||||
std::vector<ScanPoint> &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<Eigen::Vector2f> &scan) {
|
||||
std::vector<ScanPoint> points;
|
||||
calculate_normals(scan, points);
|
||||
};
|
||||
};
|
||||
95
src/simpleslam_node.cpp
Normal file
95
src/simpleslam_node.cpp
Normal file
@ -0,0 +1,95 @@
|
||||
#include <memory>
|
||||
|
||||
#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 <Eigen/Dense>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
#include "simpleslam.hpp"
|
||||
#include "utils.hpp"
|
||||
|
||||
class MinimalSubscriber : public rclcpp::Node {
|
||||
public:
|
||||
SimpleSlam slam;
|
||||
MinimalSubscriber() : Node("minimal_subscriber") {
|
||||
pub_map_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("/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<Eigen::Vector2f> scan;
|
||||
|
||||
Eigen::Vector2f min = Eigen::Vector2f(std::numeric_limits<float>::max(),
|
||||
std::numeric_limits<float>::max());
|
||||
Eigen::Vector2f max = Eigen::Vector2f(std::numeric_limits<float>::min(),
|
||||
std::numeric_limits<float>::min());
|
||||
for (size_t i = 0; i < msg->ranges.size(); i += 1) {
|
||||
auto radius = msg->ranges[i];
|
||||
Eigen::Rotation2D<float> 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<float> 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<int>();
|
||||
bresenham((-min / grid.info.resolution).cast<int>(), vi,
|
||||
[&](const auto &p) { put_p(p, 0); });
|
||||
put_p(vi, 100);
|
||||
}
|
||||
pub_map_->publish(grid);
|
||||
};
|
||||
subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
|
||||
"scan", 10, topic_callback);
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
|
||||
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_map_;
|
||||
};
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<MinimalSubscriber>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
41
src/utils.hpp
Normal file
41
src/utils.hpp
Normal file
@ -0,0 +1,41 @@
|
||||
#pragma once
|
||||
|
||||
#include <ranges>
|
||||
#include <algorithm> // For std::transform
|
||||
#include <utility> // For std::pair
|
||||
#include <Eigen/Dense>
|
||||
#include <functional>
|
||||
// Helper function to create an enumerator
|
||||
// auto enumerate(const auto& container) {
|
||||
// auto indices = std::views::iota(0, static_cast<int>(container.size()));
|
||||
// return std::views::zip(indices, container);
|
||||
// }
|
||||
void bresenham(const Eigen::Vector2i& start, const Eigen::Vector2i& end, std::function<void(const Eigen::Vector2i&)> f) {
|
||||
std::vector<Eigen::Vector2i> 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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user