From d59a488b113680646fe6aefebb3329f2114abfa5 Mon Sep 17 00:00:00 2001 From: Allan Cueff Date: Tue, 23 Jan 2024 22:32:32 +0100 Subject: [PATCH] Implementation detection points interieur de la table + CMakeLists propre --- CMakeLists.txt | 23 ++++++++++ README.md | 20 +++++++++ src/main.cpp | 112 ++++++++++++++++++++++++++++++++++++++++++++++ src/main.cpp.save | 110 +++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 265 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 src/main.cpp create mode 100644 src/main.cpp.save diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..8cf5af0 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.18.0 FATAL_ERROR) + +project(lidar) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED TRUE) +SET(CMAKE_CXX_FLAGS -pthread) + +include_directories(include) +add_executable(${PROJECT_NAME} src/main.cpp) + +set(RPLIDAR_SDK_PATH "/home/modelec/rplidar_sdk") + +add_library(rplidar STATIC IMPORTED) + +set_property(TARGET rplidar + PROPERTY IMPORTED_LOCATION ${RPLIDAR_SDK_PATH}/output/Linux/Release/libsl_lidar_sdk.a) + +target_include_directories(rplidar INTERFACE + ${RPLIDAR_SDK_PATH}/sdk/include/ + ${RPLIDAR_SDK_PATH}/sdk/src/) + +target_link_libraries(${PROJECT_NAME} rplidar) diff --git a/README.md b/README.md index 2797a3d..033bf1d 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,22 @@ # detection_adversaire Script de détection de l'adversaire par LIDAR en fonction de la position du robot + +## Dependances + +Le SDK rplidar_sdk soit être installé et compilé. Son chemin est à renseigner dans le CMakeLists.txt + +## Compilation + +cmake -B build + +cd build + +make + +## Demarrage + +Dans build : + +./lidar + + diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..f25778c --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include +#include +#include + +#ifndef _countof +#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) +#endif + +#define X_LIDAR 1.f +#define Y_LIDAR 1.f +#define ALPHA_LIDAR 0.f + +bool stop_signal_received; +void stop_loop(int) { + stop_signal_received = true; +} + +double edge_distance(double x, double y, double alpha) { + if (M_PI / 2.f > alpha && alpha >= 0.f) { + double border_right_x_distance = 3.f - x; + double border_right_y_distance = x * sin(alpha) / cos(alpha); + double border_top_x_distance = (2.f - y) * cos(alpha) / sin(alpha); + double border_top_y_distance = 2.f - y; + double distance_top = sqrt(pow(border_top_x_distance, 2) + pow(border_top_y_distance, 2)); + double distance_right = sqrt(pow(border_right_x_distance, 2) + pow(border_right_y_distance, 2)); + return std::min(distance_top, distance_right); + } else if (M_PI > alpha && alpha >= M_PI / 2.f) { + double border_top_x_distance = (2.f - y) * cos(alpha) / sin(alpha); + double border_top_y_distance = 2.f - y; + double border_left_x_distance = x; + double border_left_y_distance = (3.f - x) * sin(alpha) / cos(alpha); + double distance_top = sqrt(pow(border_top_x_distance, 2) + pow(border_top_y_distance, 2)); + double distance_left = sqrt(pow(border_left_x_distance, 2) + pow(border_left_y_distance, 2)); + return std::min(distance_top, distance_left); + } else if (M_PI * 3.f / 2.f > alpha && alpha >= M_PI) { + double border_left_x_distance = x; + double border_left_y_distance = (3.f - x) * sin(alpha) / cos(alpha); + double border_bottom_x_distance = y * cos(alpha) / sin(alpha); + double border_bottom_y_distance = y; + double distance_left = sqrt(pow(border_left_x_distance, 2) + pow(border_left_y_distance, 2)); + double distance_bottom = sqrt(pow(border_bottom_x_distance, 2) + pow(border_bottom_y_distance, 2)); + return std::min(distance_left, distance_bottom); + } else if (2.f * M_PI > alpha && alpha >= M_PI * 3.f / 2.f) { + double border_bottom_x_distance = y * cos(alpha) / sin(alpha); + double border_bottom_y_distance = y; + double border_right_x_distance = 3.f - x; + double border_right_y_distance = x * sin(alpha) / cos(alpha); + double distance_bottom = sqrt(pow(border_bottom_x_distance, 2) + pow(border_bottom_y_distance, 2)); + double distance_right = sqrt(pow(border_right_x_distance, 2) + pow(border_right_y_distance, 2)); + return std::min(distance_bottom, distance_right); + } + return 100.f; +} + +bool is_inside(double x_lidar, double y_lidar, double alpha_lidar, double distance, double angle){ + double alpha = angle / 360.f * 2.f * M_PI + alpha_lidar; + if(alpha >= (2 * M_PI)){ + alpha = alpha - 2.f * M_PI; + } + else if(alpha < 0){ + alpha = alpha + 2.f * M_PI; + } + double wall_distance = edge_distance(x_lidar, y_lidar, alpha); + return (distance < wall_distance); +} + +using namespace sl; + +int main(int argc, const char * argv[]) { + Result channel = createSerialPortChannel("/dev/ttyUSB0", 115200); + ILidarDriver * drv = *createLidarDriver(); + auto res = drv->connect(*channel); + if(SL_IS_OK(res)){ + drv->startScan(0,1); + sl_result op_result; + signal(SIGINT, stop_loop); + while(true) { + sl_lidar_response_measurement_node_hq_t nodes[8192]; + size_t count = _countof(nodes); + op_result = drv->grabScanDataHq(nodes, count); + if (SL_IS_OK(op_result)) { + drv->ascendScanData(nodes, count); + for (int pos = 0; pos < (int)count ; ++pos) { + if(nodes[pos].dist_mm_q2/4.0f !=0 && nodes[pos].quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT != 0){ + if(is_inside(X_LIDAR, Y_LIDAR, ALPHA_LIDAR, nodes[pos].dist_mm_q2/4000.0f, ((nodes[pos].angle_z_q14 * 90.f) / 16384.f))){ + printf("Detected inside echo at angle: %03.2f Dist: %08.0fmm\n", + (nodes[pos].angle_z_q14 * 90.f) / 16384.f, + nodes[pos].dist_mm_q2/4.0f + ); + } + } + } + } + if (stop_signal_received){ + break; + } + } + drv->stop(); + drv->setMotorSpeed(0); + drv->disconnect(); + } + delete *channel; + delete drv; + + drv = nullptr; + + return 0; +} diff --git a/src/main.cpp.save b/src/main.cpp.save new file mode 100644 index 0000000..5e8d66d --- /dev/null +++ b/src/main.cpp.save @@ -0,0 +1,110 @@ +#include +#include +#include +#include +#include +#include +#include + +#ifndef _countof +#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) +#endif + +#define X_LIDAR 1 +#define Y_LIDAR 1 +#define ALPHA_LIDAR 0 + +bool stop_signal_received; +void stop_loop(int) { + stop_signal_received = true; +} + +double edge_distance(double x, double y, double alpha) { + if (M_PI / 2 > alpha && alpha > 0) { + double border_right_x_distance = 3 - x; + double border_right_y_distance = (0 - x) * sin(alpha) / cos(alpha); + double border_top_x_distance = (2 - y) * cos(alpha) / sin(alpha); + double border_top_y_distance = 2 - y; + double distance_top = sqrt(pow(border_top_x_distance, 2) + pow(border_top_y_distance, 2)); + double distance_right = sqrt(pow(border_right_x_distance, 2) + pow(border_right_y_distance, 2)); + return std::min(distance_top, distance_right); + } else if (M_PI > alpha && alpha > M_PI / 2) { + double border_top_x_distance = (2 - y) * cos(alpha) / sin(alpha); + double border_top_y_distance = 2 - y; + double border_left_x_distance = x; + double border_left_y_distance = (3 - x) * sin(alpha) / cos(alpha); + double distance_top = sqrt(pow(border_top_x_distance, 2) + pow(border_top_y_distance, 2)); + double distance_left = sqrt(pow(border_left_x_distance, 2) + pow(border_left_y_distance, 2)); + return std::min(distance_top, distance_left); + } else if (M_PI * 3 / 2 > alpha && alpha > M_PI) { + double border_left_x_distance = x; + double border_left_y_distance = (3 - x) * sin(alpha) / cos(alpha); + double border_bottom_x_distance = y * cos(alpha) / sin(alpha); + double border_bottom_y_distance = y; + double distance_left = sqrt(pow(border_left_x_distance, 2) + pow(border_left_y_distance, 2)); + double distance_bottom = sqrt(pow(border_bottom_x_distance, 2) + pow(border_bottom_y_distance, 2)); + return std::min(distance_left, distance_bottom); + } else if (2 * M_PI > alpha && alpha > M_PI * 3 / 2) { + double border_bottom_x_distance = y * cos(alpha) / sin(alpha); + double border_bottom_y_distance = y; + double border_right_x_distance = 3 - x; + double border_right_y_distance = x * sin(alpha) / cos(alpha); + double distance_bottom = sqrt(pow(border_bottom_x_distance, 2) + pow(border_bottom_y_distance, 2)); + double distance_right = sqrt(pow(border_right_x_distance, 2) + pow(border_right_y_distance, 2)); + return std::min(distance_bottom, distance_right); + } +} + +bool is_inside(double x_lidar, double y_lidar, double alpha_lidar, double distance, double angle){ + double alpha = alpha_lidar + angle; + if(alpha > 2 * M_PI){ + alpha = alpha - 2 * M_PI; + } + else if(alpha < 0){ + alpha = alpha + 2 * M_PI; + } + return distance < edge_distance(x_lidar, y_lidar, alpha); +} + +using namespace sl; + +int main(int argc, const char * argv[]) { + Result channel = createSerialPortChannel("/dev/ttyUSB0", 115200); + ILidarDriver * drv = *createLidarDriver(); + auto res = drv->connect(*channel); + if(SL_IS_OK(res)){ + drv->startScan(0,1); + sl_result op_result; + signal(SIGINT, stop_loop); + while(true) { + sl_lidar_response_measurement_node_hq_t nodes[8192]; + size_t count = _countof(nodes); + op_result = drv->grabScanDataHq(nodes, count); + if (SL_IS_OK(op_result)) { + drv->ascendScanData(nodes, count); + for (int pos = 0; pos < (int)count ; ++pos) { + if(nodes[pos].dist_mm_q2/4.0f != 0){ + if(is_inside(X_LIDAR, Y_LIDAR, ALPHA_LIDAR, nodes[pos].dist_mm_q2/1000/4.0f, (nodes[pos].angle_z_q14 * 90.f) / 16384.f)){ + printf("Detected inside : %03.2f Dist: %08.2f Q: %d \n", + (nodes[pos].angle_z_q14 * 90.f) / 16384.f, + nodes[pos].dist_mm_q2/4.0f, + nodes[pos].quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_S + } + } + } + } + if (stop_signal_received){ + break; + } + } + drv->stop(); + drv->setMotorSpeed(0); + drv->disconnect(); + } + delete *channel; + delete drv; + + drv = nullptr; + + return 0; +}