Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Added lidar opponent tracking

Browse Source
This commit is contained in:
Pimpest
2025-05-06 03:09:12 +02:00
committed by Pimpest
parent abf5717286
commit 50106a439b
4 changed files with 223 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
mg_lidar/CMakeLists.txt Normal file
View File

@ -0,0 +1,61 @@
cmake_minimum_required(VERSION 3.12)
project(mg_lidar)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
set(PACKAGE_DEPS
rclcpp
tf2
tf2_ros
tf2_geometry_msgs
mg_msgs
sensor_msgs
)
find_package(ament_cmake REQUIRED)
include(FindPkgConfig)
pkg_search_module(LIBGLM REQUIRED glm)
foreach(PACKAGE ${PACKAGE_DEPS})
find_package(${PACKAGE} REQUIRED)
endforeach()
set(SOURCES
src/scanner.cpp
)
add_executable(mg_scanner ${SOURCES})
target_include_directories(
mg_scanner
PRIVATE
${LIBGLM_INCLUDE_DIRS}
include
)
ament_target_dependencies(mg_scanner ${PACKAGE_DEPS})
install( TARGETS
mg_scanner
DESTINATION lib/${PROJECT_NAME}
)
# install(DIRECTORY
# mg_scanner
# config
# launch
# DESTINATION share/${PROJECT_NAME}/
# )
target_compile_features(mg_scanner PUBLIC
c_std_99
cxx_std_17
) # Require C99 and C++17
ament_package()

27
mg_lidar/package.xml Normal file
View File

@ -0,0 +1,27 @@
<?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>mg_lidar</name>
<version>0.0.0</version>
<description>Lidar based opponent tracking</description>
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
<license>Apache 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>mg_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>visualization_msgs</depend>
<depend>sensor_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

134
mg_lidar/src/scanner.cpp Normal file
View File

@ -0,0 +1,134 @@
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "glm/glm.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/convert.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include <glm/gtx/norm.hpp>
class MgScanner : public rclcpp::Node {
using LaserScan = sensor_msgs::msg::LaserScan;
using PointStamped = geometry_msgs::msg::PointStamped;
public:
MgScanner() : rclcpp::Node("EnemyScanner") {
gen_rotations();
scan_sup_ = create_subscription<LaserScan>(
"scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { process_scan(msg); });
enemy_pub_ = create_publisher<PointStamped>("/dynamicObstacle", rclcpp::QoS(1));
tf_buf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
}
private:
rclcpp::Subscription<LaserScan>::SharedPtr scan_sup_;
rclcpp::Publisher<PointStamped>::SharedPtr enemy_pub_;
std::shared_ptr<tf2_ros::Buffer> tf_buf_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::vector<glm::dmat2> rotations;
void gen_rotations() {
constexpr double min_angle = -3.1241393089294434;
constexpr double max_angle = 3.1415927410125732;
constexpr double angle_increment = 0.008714509196579456;
double curr = min_angle;
while (curr <= max_angle) {
rotations.emplace_back(glm::cos(curr), glm::sin(curr), -glm::sin(curr), glm::cos(curr));
curr += angle_increment;
}
rotations.emplace_back(glm::cos(curr), glm::sin(curr), -glm::sin(curr), glm::cos(curr));
}
glm::dvec3 pos_query() {
RCLCPP_ERROR(get_logger(), "Works to here");
try {
double x = NAN;
double y = NAN;
double rot = NAN;
auto tmsg = tf_buf_->lookupTransform("odom", "base-link", tf2::TimePointZero);
tf2::Transform t;
tf2::convert(tmsg.transform, t);
t.getBasis().getRPY(x, y, rot);
auto vec3 = tmsg.transform.translation;
return { vec3.x, vec3.y, rot };
} catch (const tf2::TransformException& e) {
RCLCPP_ERROR(get_logger(), "Got following error when looking up transform:\n\t%s", e.what());
}
return { 0, 0, 0 };
}
static bool part_of_clump(const glm::dvec2 prev, glm::dvec2 curr) {
return glm::length2(prev - curr) < 0.05 * 0.05;
}
void process_scan(LaserScan::ConstSharedPtr msg) {
// TODO: finish processing
const glm::dvec3 v = pos_query();
const glm::dvec2 pos = { v.x, v.y };
const glm::dmat2 rot = { glm::cos(v.z), glm::sin(v.z), -glm::sin(v.z), glm::cos(v.z) };
double mini = INFINITY;
glm::dvec2 mini_pos = { 0, 0 };
glm::dvec2 prev = { -1, -1 };
glm::dvec2 clump = { -1, -1 };
int clump_size = 0;
bool good_clump = false;
if (msg->ranges.size() != rotations.size()) {
RCLCPP_ERROR(get_logger(),
"The amount of rotations differs from amount of ranges(%lu != %lu)",
msg->ranges.size(),
rotations.size());
}
for (uint i = 0; i < msg->ranges.size(); i++) {
if (msg->intensities.at(i) < 35.0) {
continue;
}
const glm::dvec2 potential_pos = rot * rotations.at(i) * glm::dvec2{ msg->ranges.at(i), 0 } + pos;
if (0.1 < potential_pos.x && potential_pos.x < 2.9 && 0.1 < potential_pos.y && potential_pos.y < 1.9) {
if (!part_of_clump(prev, potential_pos)) {
clump_size = 0;
clump = { 0, 0 };
}
good_clump |= mini > msg->ranges.at(i);
clump += potential_pos;
clump_size++;
if (good_clump) {
mini = msg->ranges.at(i);
mini_pos = clump / (double)clump_size;
}
}
prev = potential_pos;
}
if (mini < INFINITY) {
PointStamped m;
m.header.frame_id = "odom";
m.point.x = mini_pos.x;
m.point.y = mini_pos.y;
enemy_pub_->publish(m);
}
}
};
int main(const int argc, const char** argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MgScanner>());
rclcpp::shutdown();
}

2
mg_obstacles/src/mg_obstacles.cpp
View File

@ -153,7 +153,7 @@ namespace mg {
double ObstacleManager::dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rot) {
// Find me
const glm::dmat2 rot_mat{ { glm::cos(rot), -glm::sin(rot) }, { glm::sin(rot), glm::cos(rot) } };
const glm::dmat2 rot_mat{ { glm::cos(rot), glm::sin(rot) }, { -glm::sin(rot), glm::cos(rot) } };
return ObstacleManager::dist_to_bounds(pos, size, rot_mat);
}