diff --git a/.gitignore b/.gitignore index 5047c6cb..12f1c208 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ +vsxmake* + docs/* !docs/index.html !docs/README.md diff --git a/backend/src/App.cpp b/backend/src/App.cpp index c188eff6..bbdcb62f 100644 --- a/backend/src/App.cpp +++ b/backend/src/App.cpp @@ -3,6 +3,7 @@ #include "oatpp/network/Server.hpp" +#include "cartographer/mapping/2d/" #include void run() { diff --git a/backend/xmake.lua b/backend/xmake.lua index a56143fc..a0c7bd01 100644 --- a/backend/xmake.lua +++ b/backend/xmake.lua @@ -9,6 +9,7 @@ set_languages("cxx17") add_requires("boost", { configs = {chrono = true} }) add_requires("fmt", "spdlog") add_requires("nlohmann_json") +add_requires("vcpkg::cartographer", {alias = "cartographer"}) -- C++ Backend API for Svelte App add_requires("oatpp", "oatpp-websocket") @@ -25,6 +26,8 @@ target("backend") add_packages("oatpp", "oatpp-websocket") add_packages("tl_expected") + add_packages("cartographer") + before_build_files(function(target) local frontend_dir = path.join(os.scriptdir(), "..", "frontend") os.execv("pnpm", {"--prefix", frontend_dir, "i"}) diff --git a/lidar/.gitignore b/lidar/.gitignore new file mode 100644 index 00000000..15210576 --- /dev/null +++ b/lidar/.gitignore @@ -0,0 +1,8 @@ +# Xmake cache +.xmake/ +build/ + +# MacOS Cache +.DS_Store + + diff --git a/lidar/src/main.cpp b/lidar/src/main.cpp new file mode 100644 index 00000000..f389f854 --- /dev/null +++ b/lidar/src/main.cpp @@ -0,0 +1,93 @@ +#include +#include +#include +#include +#include +#include "cartographer/mapping/proto/map_builder_options.pb.h" +#include "RPLidar.h" + +#include + +// Edited code from "https://github.com/cartographer-project/cartographer_ros" +std::tuple LoadOptions( + const std::string& configuration_directory, + const std::string& configuration_basename) { + auto file_resolver = + std::make_unique( + std::vector{configuration_directory}); + const std::string code = + file_resolver->GetFileContentOrDie(configuration_basename); + cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver)); + + return std::make_tuple(CreateMapBuilderOptions(&lua_parameter_dictionary), + CreateTrajectoryBuilderOptions(&lua_parameter_dictionary)); +} + +cartographer::mapping::proto::MapBuilderOptions CreateMapBuilderOptions( + ::cartographer::common::LuaParameterDictionary* const + lua_parameter_dictionary) { + return + ::cartographer::mapping::CreateMapBuilderOptions( + lua_parameter_dictionary->GetDictionary("map_builder").get()); +} + +cartographer::mapping::proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( + ::cartographer::common::LuaParameterDictionary* const + lua_parameter_dictionary) { + return + ::cartographer::mapping::CreateTrajectoryBuilderOptions( + lua_parameter_dictionary->GetDictionary("trajectory_builder").get()); +} + +int main() { + cartographer::mapping::proto::MapBuilderOptions map_builder_options; + cartographer::mapping::proto::TrajectoryBuilderOptions trajectory_builder; + std::tie(map_builder_options, trajectory_builder) = LoadOptions("", ""); + + cartographer::mapping::MapBuilder map_builder(map_builder_options); + + std::set sensor_ids; + + sensor_ids.insert(cartographer::mapping::TrajectoryBuilderInterface::SensorId{ + cartographer::mapping::TrajectoryBuilderInterface::SensorId::SensorType::LOCAL_SLAM_RESULT, + "", + }); + const int trajectory_id = map_builder.AddTrajectoryBuilder(sensor_ids, trajectory_builder, [](const int trajectory_id, const ::cartographer::common::Time time, + const cartographer::transform::Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local, + const std::unique_ptr< + const ::cartographer::mapping::TrajectoryBuilderInterface:: + InsertionResult>) { + }); + // Initialize and start your lidar sensor here (e.g., using your_lidar_library). + //RPLidar lidar; + + //while (true) { + // // Read lidar data in real-time. + // std::vector lidar_angles; // List of angles (in radians) + // std::vector lidar_distances; // List of corresponding distances (in meters) + + // lidar.readData(lidar_angles, lidar_distances); + + // cartographer::common::Time time = cartographer::common::FromUniversal(0); + // Eigen::Vector3d sensor_pose(0.0, 0.0, 0.0); + + // cartographer::sensor::TimedPointCloudData point_cloud_data( + // time, sensor_pose, cartographer::sensor::PointCloud{}); + + // for (size_t i = 0; i < lidar_angles.size(); ++i) { + // Eigen::Vector3f point(lidar_distances[i] * std::cos(lidar_angles[i]), + // lidar_distances[i] * std::sin(lidar_angles[i]), 0.0); + // point_cloud_data.point_cloud.push_back(point); + // } + + // map_builder_options.AddSensorData(trajectory_id, point_cloud_data); + + // map_builder_options.FinishTrajectory(); + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + //} + + + return 0; +} diff --git a/lidar/xmake.lua b/lidar/xmake.lua new file mode 100644 index 00000000..a7914f9e --- /dev/null +++ b/lidar/xmake.lua @@ -0,0 +1,18 @@ +add_rules("mode.debug", "mode.release") + +add_requires("vcpkg::cartographer", {alias = "cartographer"}) + +add_repositories("my-repo repository", {rootdir = path.join(os.scriptdir(), "..")}) +add_requires("rplidar") + +target("lidar") + set_kind("binary") + + add_packages("cartographer") + + add_packages("rplidar") + + add_files("src/*.cpp") + + add_defines("NOMINMAX") + add_defines("GLOG_NO_ABBREVIATED_SEVERITIES") diff --git a/raspberry_pi/SETUP.md b/raspberry_pi/SETUP.md index f0c3ba91..6d782f1c 100644 --- a/raspberry_pi/SETUP.md +++ b/raspberry_pi/SETUP.md @@ -1,3 +1,7 @@ Set up the SSH, Wifi of the Raspberry Pi first. -Run the following command to get XMake: `curl -fsSL https://xmake.io/shget.text | bash` \ No newline at end of file +Run the following command to get XMake: `curl -fsSL https://xmake.io/shget.text | bash` + +Run the following command to get Docker: `curl -fsSL https://get.docker.com -o get-docker.sh && sudo sh get-docker.sh` + +Run the following command to get image: `sudo docker pull ros:noetic` and `sudo docker run -it ros:noetic` \ No newline at end of file diff --git a/repository/packages/r/rplidar/rplidar/include/ExpressPacket.hpp b/repository/packages/r/rplidar/rplidar/include/ExpressPacket.hpp new file mode 100644 index 00000000..f720718f --- /dev/null +++ b/repository/packages/r/rplidar/rplidar/include/ExpressPacket.hpp @@ -0,0 +1,53 @@ +#ifndef EXPRESSPACKET_H +#define EXPRESSPACKET_H + +#pragma once + +// Made with the help of ChatGPT + +#include +#include +#include + +class ExpressPacket { +public: + static const uint8_t sync1 = 0xa; + static const uint8_t sync2 = 0x5; + + ExpressPacket(std::vector data) { + if ((data[0] >> 4) != sync1 || (data[1] >> 4) != sync2) { + throw std::invalid_argument("try to parse corrupted data"); + } + + uint8_t checksum = 0; + for (size_t i = 2; i < data.size(); i++) { + checksum ^= data[i]; + } + + if (checksum != ((data[0] & 0x0F) + ((data[1] & 0x0F) << 4))) { + throw std::invalid_argument("Invalid checksum"); + } + + new_scan = (data[2] >> 7) & 0x01; + start_angle = static_cast((data[1] & 0x0F) << 8 | data[2]) / 64.0f; + + for (size_t i = 4; i < data.size(); i += 5) { + distance.push_back(((data[i + 1] >> 2) & 0x3F) | ((data[i] & 0x3F) << 6)); + angle.push_back((((data[i + 3] & 0x0F) + ((data[i + 1] & 0x01) << 4)) / 8.0f) * getSign(data[i + 1])); + distance.push_back(((data[i + 2] >> 2) & 0x3F) | ((data[i + 1] & 0x3F) << 6)); + angle.push_back((((data[i + 3] >> 4) & 0x0F) + ((data[i + 2] & 0x01) << 4)) / 8.0f * getSign(data[i + 2])); + } + } + + static int getSign(uint8_t value) { + return (value & 0x02) ? -1 : 1; + } + +public: + std::vector distance; + std::vector angle; + bool new_scan; + float start_angle; +}; + +#endif \ No newline at end of file diff --git a/repository/packages/r/rplidar/rplidar/include/RPLidar.h b/repository/packages/r/rplidar/rplidar/include/RPLidar.h new file mode 100644 index 00000000..524e792d --- /dev/null +++ b/repository/packages/r/rplidar/rplidar/include/RPLidar.h @@ -0,0 +1,168 @@ +#ifndef RPLIDAR_H +#define RPLIDAR_H + +#pragma once + +// Made with the help of ChatGPT + +#include +#include + +#include + +#include +#include +#include + +// std::this_thread::sleep_for(std::chrono::milliseconds(1)); +#include +#include + +// convertToHexString +#include +#include + +// iter_measures +#include + +#include +#include + +#include "ExpressPacket.hpp" + +#define SYNC_BYTE 0xA5 +#define SYNC_BYTE2 0x5A + +#define GET_INFO_BYTE 0x50 +#define GET_HEALTH_BYTE 0x52 + +#define STOP_BYTE 0x25 +#define RESET_BYTE 0x40 + +enum ScanType +{ + NORMAL, + FORCE, + EXPRESS +}; + +struct ScanInfo +{ + int currently_scanning; + int dsize; + ScanType type; +}; + +static std::map> SCAN_TYPE = { + {ScanType::NORMAL, {{"byte", 0x20}, {"response", 129}, {"size", 5}}}, + {ScanType::FORCE, {{"byte", 0x21}, {"response", 129}, {"size", 5}}}, + {ScanType::EXPRESS, {{"byte", 0x82}, {"response", 130}, {"size", 84}}}}; + +#define DESCRIPTOR_LEN 7 +#define INFO_LEN 20 +#define HEALTH_LEN 3 + +#define INFO_TYPE 4 +#define HEALTH_TYPE 6 + +#define MAX_MOTOR_PWM 1023 +#define DEFAULT_MOTOR_PWM 660 +#define SET_PWM_BYTE 0xF0 + +static std::map HEALTH_STATUSES = { + {0, "Good"}, + {1, "Warning"}, + {2, "Error"}}; + +struct DeviceInfo +{ + uint8_t model; + std::pair firmware; + uint8_t hardware; + std::string serialNumber; +}; + +/** + * @brief + * Health Info for Lidar Scanner + */ +struct HealthInfo +{ + /** + * @brief + * 'Good', 'Warning' or 'Error' statuses + */ + std::string status; + /** + * @brief + * The related error code that caused a warning/error. + */ + int errorCode; +}; + +struct Measure +{ + bool newScan; + int quality; + double angle; + double distance; +}; + +/** + * @brief Class for communicating with RPLidar rangefinder scanners + * + */ +class RPLidar +{ +public: + RPLidar(const std::string &port, uint32_t baudrate = 115200U); + + ~RPLidar(); + + void disconnect(); + + void _set_pwm(int pwm); + + void set_motor_speed(int pwm); + + void start_motor(); + + void stop_motor(); + + void _send_payload_cmd(uint8_t cmd, const std::string &payload); + + void _send_cmd(uint8_t cmd); + + std::tuple _read_descriptor(); + + std::vector _read_response(int dsize); + + DeviceInfo get_info(); + + HealthInfo get_health(); + + void clean_input(); + + void stop(); + + void start(ScanType scanType); + + void reset(); + + std::function iter_measures(ScanType scanType = NORMAL, int maxBufMeas = 3000); + + std::function()> iter_scans(ScanType scanType = NORMAL, int maxBufMeas = 3000, int minLen = 5); + +private: + std::unique_ptr _serial = nullptr; + std::string port; + uint32_t baudrate; + int _motor_speed = DEFAULT_MOTOR_PWM; + bool motor_running = false; + ScanInfo scanning = {false, 0, ScanType::NORMAL}; + int express_trame = 32; + std::unique_ptr express_data = nullptr; + std::unique_ptr express_old_data = nullptr; +}; + +#endif \ No newline at end of file diff --git a/repository/packages/r/rplidar/rplidar/src/RPLidar.cpp b/repository/packages/r/rplidar/rplidar/src/RPLidar.cpp new file mode 100644 index 00000000..4f634677 --- /dev/null +++ b/repository/packages/r/rplidar/rplidar/src/RPLidar.cpp @@ -0,0 +1,607 @@ +#include "RPLidar.h" + +// Made with the help of ChatGPT + +/* +auto lidar = RPLidar("COM3"); + +auto info = lidar.get_info(); +std::cout << fmt::format("model: {}, firmware: ({}, {}), hardware: {}, serialnumber: {}\n", info.model, info.firmware.first, info.firmware.second, info.hardware, info.serialNumber); + +auto health = lidar.get_health(); +std::cout << fmt::format("({}, {})\n", health.status, health.errorCode); + +std::function()> scanGenerator = lidar.iter_scans(); +for (int i = 0; i < 10; i++) +{ + std::vector scan = scanGenerator(); + std::cout << "Got " << scan.size() << " Measures!\n"; + for (const Measure &measure : scan) + { + // Access individual measurements in the scan + bool newScan = measure.newScan; + int quality = measure.quality; + float angle = measure.angle; + float distance = measure.distance; + } +} + +lidar.stop(); +lidar.stop_motor(); +lidar.disconnect(); + +*/ + +/** + * @brief Initilize RPLidar object for communicating with the sensor + * + * @param port Serial port name to which sensor is connected + * @param baudrate Baudrate for serial connection (the default is 115200) + */ +RPLidar::RPLidar(const std::string &port, uint32_t baudrate) +{ + this->port = port; + this->baudrate = baudrate; + try + { + this->_serial = std::make_unique(port, baudrate, serial::Timeout(1000U)); + std::cout << "Opened serial: " << _serial->isOpen() << '\n'; + } + catch (std::exception &e) + { + std::cout << "Unable to open serial: " << e.what() << '\n'; + } +} + +RPLidar::~RPLidar() +{ +} + +void RPLidar::disconnect() +{ + if (!this->_serial->isOpen()) + { + return; + } + this->_serial->close(); +} + +void RPLidar::_set_pwm(int pwm) +{ + std::string payload; + payload.push_back(static_cast(pwm & 0xFF)); + payload.push_back(static_cast((pwm >> 8) & 0xFF)); + this->_send_payload_cmd(SET_PWM_BYTE, payload); +} + +void RPLidar::set_motor_speed(int pwm) +{ + if (0 <= pwm && pwm <= MAX_MOTOR_PWM) + { + std::abort(); + } + this->_motor_speed = pwm; + if (this->motor_running) + this->_set_pwm(this->_motor_speed); +} + +/** + * @brief Starts sensor motor + * + */ +void RPLidar::start_motor() +{ + spdlog::info("Starting motor"); + // For A1 + this->_serial->setDTR(false); + + // For A2 + this->_set_pwm(this->_motor_speed); + this->motor_running = true; +} + +/** + * @brief Stops sensor motor + * + */ +void RPLidar::stop_motor() +{ + spdlog::info("Stopping motor"); + // For A2 + this->_set_pwm(0); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + + // For A1 + this->_serial->setDTR(true); + this->motor_running = false; +} + +/** + * @brief Sends `cmd` command with `payload` to the sensor + * + * @param cmd + * @param payload + */ +void RPLidar::_send_payload_cmd(uint8_t cmd, const std::string &payload) +{ + // Calculate the size + uint8_t size = static_cast(payload.size()); + + // Construct the request string + std::string req; + req += static_cast(SYNC_BYTE); + req += static_cast(cmd); + req += static_cast(size); + req += payload; + + // Calculate the checksum + uint8_t checksum = 0; + for (const char &c : req) + { + checksum ^= static_cast(c); + } + + req += static_cast(checksum); + + this->_serial->write(req); + spdlog::debug("Command sent: {}", spdlog::to_hex(req)); +} + +/** + * @brief Sends `cmd` command to the sensor + * + * @param cmd + */ +void RPLidar::_send_cmd(uint8_t cmd) +{ + std::string req; + req += static_cast(SYNC_BYTE); + req += static_cast(cmd); + + this->_serial->write(req); + spdlog::debug("Command sent: {}", spdlog::to_hex(req)); +} + +/** + * @brief Reads descriptor packet + * + * @return std::tuple + */ +std::tuple RPLidar::_read_descriptor() +{ + // Read descriptor packet + std::vector descriptor(DESCRIPTOR_LEN); + this->_serial->read(descriptor.data(), DESCRIPTOR_LEN); + spdlog::debug("Received descriptor: {}", spdlog::to_hex(descriptor)); + + if (descriptor.size() != DESCRIPTOR_LEN) + { + throw std::runtime_error("Descriptor length mismatch"); + } + else if (descriptor[0] != SYNC_BYTE || descriptor[1] != SYNC_BYTE2) + { + throw std::runtime_error("Incorrect descriptor starting bytes"); + } + + bool isSingle = descriptor[5] == 0; + return std::make_tuple(descriptor[2], isSingle, descriptor[6]); +} + +/** + * @brief Reads response packet with length of `dsize` bytes + * + * @param dsize + * @return std::vector + */ +std::vector RPLidar::_read_response(int dsize) +{ + spdlog::debug("Trying to read response: {} bytes", dsize); + + std::vector data; + data.reserve(dsize); + + while (this->_serial->available() < dsize) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + this->_serial->read(data, dsize); + + spdlog::debug("Received data: {}", spdlog::to_hex(data)); + return data; +} + +std::string convertToHexString(uint8_t value) +{ + // Convert a uint8_t to a hexadecimal string + std::stringstream stream; + stream << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << static_cast(value); + return stream.str(); +} + +/** + * @brief Get device information + * + * @return DeviceInfo + */ +DeviceInfo RPLidar::get_info() +{ + // Check if there's data in the buffer + if (this->_serial->available() > 0) + { + throw std::runtime_error("Data in buffer, you can't have info! Run flush() to empty the buffer."); + } + this->_send_cmd(GET_INFO_BYTE); + + uint8_t dsize; + bool isSingle; + uint8_t dtype; + std::tie(dsize, isSingle, dtype) = this->_read_descriptor(); + + // Check response properties + if (dsize != INFO_LEN) + { + throw std::runtime_error("Wrong get_info reply length"); + } + if (!isSingle) + { + throw std::runtime_error("Not a single response mode"); + } + if (dtype != INFO_TYPE) + { + throw std::runtime_error("Wrong response data type"); + } + + // Read the response + std::vector raw = this->_read_response(dsize); + + // Convert serial number to a hex string + std::string serialNumber; + for (size_t i = 4; i < raw.size(); ++i) + { + serialNumber += convertToHexString(raw[i]); + } + + // Construct the device info struct + DeviceInfo info; + info.model = raw[0]; + info.firmware.first = raw[2]; + info.firmware.second = raw[1]; + info.hardware = raw[3]; + info.serialNumber = serialNumber; + + return info; +} + +/** + * @brief Get device health state. When the core system detects some + potential risk that may cause hardware failure in the future, + the returned status value will be 'Warning'. But sensor can still work + as normal. When sensor is in the Protection Stop state, the returned + status value will be 'Error'. In case of warning or error statuses + non-zero error code will be returned. + * + * @return HealthInfo + */ +HealthInfo RPLidar::get_health() +{ + // Check if there's data in the buffer + if (this->_serial->available() > 0) + { + throw std::runtime_error("Data in buffer, you can't get health info! Run cleanInput() to empty the buffer."); + } + spdlog::info("Asking for health"); + + this->_send_cmd(GET_HEALTH_BYTE); + + // Read the descriptor + uint8_t dsize; + bool isSingle; + uint8_t dtype; + std::tie(dsize, isSingle, dtype) = this->_read_descriptor(); + + // Check response properties + if (dsize != HEALTH_LEN) + { + throw std::runtime_error("Wrong get_health reply length"); + } + if (!isSingle) + { + throw std::runtime_error("Not a single response mode"); + } + if (dtype != HEALTH_TYPE) + { + throw std::runtime_error("Wrong response data type"); + } + + // Read the response + std::vector raw = this->_read_response(dsize); + + // Extract status and error code + std::string status = HEALTH_STATUSES[raw[0]]; + int errorCode = (static_cast(raw[1]) << 8) + static_cast(raw[2]); + + return {status, errorCode}; +} + +/** + * @brief + * Clean input buffer by reading all available data + */ +void RPLidar::clean_input() +{ + if (this->scanning.currently_scanning) + { + throw std::runtime_error("Cleaning not allowed during scanning process active!"); + } + this->_serial->flushInput(); + this->express_trame = 32; + this->express_data = nullptr; +} + +/** + * @brief + * Stops scanning process, disables laser diode and the measurement + system, moves sensor to the idle state. + */ +void RPLidar::stop() +{ + spdlog::info("Stopping scanning"); + this->_send_cmd(STOP_BYTE); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + this->scanning.currently_scanning = false; + this->clean_input(); +} + +/** + * @brief Start the scanning process + * + * @param scanType NORMAL, FORCE or EXPRESS + */ +void RPLidar::start(ScanType scanType = NORMAL) +{ + if (this->scanning.currently_scanning) + { + throw std::runtime_error("Scanning already running!"); + } + + HealthInfo healthInfo = this->get_health(); + std::string status = healthInfo.status; + int errorCode = healthInfo.errorCode; + spdlog::debug("Health status: {} [{}}]", status, errorCode); + + if (status == "Error") + { + this->reset(); + healthInfo = this->get_health(); + status = healthInfo.status; + errorCode = healthInfo.errorCode; + if (status == "Error") + { + throw std::runtime_error("RPLidar hardware failure. Error code: " + std::to_string(errorCode)); + } + } + else if (status == "Warning") + { + spdlog::warn("Warning sensor status detected! Error code: {}", errorCode); + } + + uint8_t cmd = SCAN_TYPE[scanType]["byte"]; + + spdlog::warn("starting scan process in {} mode", scanType); + + if (scanType == EXPRESS) + { + // Adjust this part according to your payload format + std::string payload = "\x00\x00\x00\x00\x00"; + this->_send_payload_cmd(cmd, payload); + } + else + { + this->_send_cmd(cmd); + } + + uint8_t dsize; + bool isSingle; + uint8_t dtype; + std::tie(dsize, isSingle, dtype) = this->_read_descriptor(); + + if (dsize != SCAN_TYPE[scanType]["size"]) + { + throw std::runtime_error("Wrong get_info reply length"); + } + if (isSingle) + { + throw std::runtime_error("Not a multiple response mode"); + } + if (dtype != SCAN_TYPE[scanType]["response"]) + { + throw std::runtime_error("Wrong response data type"); + } + + scanning = {true, dsize, scanType}; +} + +/** + * @brief Resets sensor core, reverting it to a similar state as it has + just been powered up. + * + */ +void RPLidar::reset() +{ + spdlog::info("Resetting the sensor"); + this->_send_cmd(RESET_BYTE); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + this->clean_input(); +} + +Measure _process_scan(const std::vector &raw) +{ + Measure measurementData; + + bool newScan = static_cast(raw[0] & 0b1); + bool inversedNewScan = static_cast((raw[0] >> 1) & 0b1); + int quality = static_cast(raw[0] >> 2); + + if (newScan == inversedNewScan) + { + throw std::runtime_error("New scan flags mismatch"); + } + + int checkBit = static_cast(raw[1] & 0b1); + if (checkBit != 1) + { + throw std::runtime_error("Check bit not equal to 1"); + } + + int anglePart1 = static_cast(raw[1] >> 1); + int anglePart2 = static_cast(raw[2]) << 7; + float angle = (anglePart1 + anglePart2) / 64.0; + + int distancePart1 = static_cast(raw[3]); + int distancePart2 = static_cast(raw[4]) << 8; + float distance = (distancePart1 + distancePart2) / 4.0; + + measurementData.newScan = newScan; + measurementData.quality = quality; + measurementData.angle = angle; + measurementData.distance = distance; + + return measurementData; +} + +Measure _process_express_scan(std::unique_ptr &data, float newAngle, int trame) +{ + Measure measurementData; + + bool newScan = (newAngle < data->start_angle) && (trame == 1); + + float angle = std::fmod((data->start_angle + ((newAngle - data->start_angle) / 32 * trame - data->angle[trame - 1])), 360); + float distance = data->distance[trame - 1]; + + measurementData.newScan = newScan; + measurementData.quality = 0; // Replace this with your actual quality value + measurementData.angle = angle; + measurementData.distance = distance; + + return measurementData; +} + +/** + * @brief Iterate over measures. Note that consumer must be fast enough, + otherwise data will be accumulated inside buffer and consumer will get + data with increasing lag. + * + * @param scanType + * @param maxBufMeas int or False if you want unlimited buffer + Maximum number of bytes to be stored inside the buffer. Once + numbe exceeds this limit buffer will be emptied out. + * @return std::function + */ +std::function RPLidar::iter_measures(ScanType scanType, int maxBufMeas) +{ + this->start_motor(); + + if (!this->scanning.currently_scanning) + { + this->start(scanType); + } + + // Define a lambda function to generate measures + auto generator = [this, scanType, maxBufMeas]() -> Measure + { + while (true) + { + int dsize = scanning.dsize; + + if (maxBufMeas) + { + int dataInBuf = this->_serial->available(); + if (dataInBuf > maxBufMeas) + { + spdlog::warn( + "Too many bytes in the input buffer: {}/{}. \n" + "Cleaning buffer...", + dataInBuf, maxBufMeas); + this->stop(); + this->start(scanning.type); + } + } + + if (scanType == NORMAL) + { + std::vector raw = this->_read_response(dsize); + Measure measure = _process_scan(raw); + return measure; + } + else if (scanType == EXPRESS) + { + if (this->express_trame == 32) + { + this->express_trame = 0; + + if (this->express_data == nullptr) + { + spdlog::debug("reading first time bytes"); + this->express_data = std::make_unique(ExpressPacket(this->_read_response(dsize))); + } + + this->express_old_data = std::move(this->express_data); + spdlog::debug("set old_data with start_angle {}", this->express_old_data->start_angle); + this->express_data = std::make_unique(ExpressPacket(this->_read_response(dsize))); + spdlog::debug("set new_data with start_angle {}", this->express_data->start_angle); + } + this->express_trame++; + spdlog::debug("process scan of frame %d with angle : \n" + "%f and angle new : %f", this->express_trame, + this->express_old_data->start_angle, + this->express_data->start_angle); + Measure measure = _process_express_scan(this->express_old_data, this->express_data->start_angle, this->express_trame); + return measure; + } + } + }; + + return generator; +} + +/** + * @brief Iterate over scans. Note that consumer must be fast enough, + otherwise data will be accumulated inside buffer and consumer will get + data with increasing lag. + * + * @param scanType + * @param maxBufMeas Maximum number of measures to be stored inside the buffer. Once + numbe exceeds this limit buffer will be emptied out. + * @param minLen Minimum number of measures in the scan for it to be yelded. + * @return std::function()> + */ +std::function()> RPLidar::iter_scans(ScanType scanType, int maxBufMeas, int minLen) +{ + std::vector scanList; + std::function measureIterator = this->iter_measures(scanType, maxBufMeas); + + // Define a lambda function to generate scans + auto scanGenerator = [this, scanType, maxBufMeas, minLen, measureIterator]() -> std::vector + { + std::vector scanList; + for (;;) + { + Measure measure = measureIterator(); + bool newScan = measure.newScan; + if (newScan) + { + if (scanList.size() > minLen) + { + return scanList; + } + scanList.clear(); + } + if (measure.distance > 0) + { + scanList.push_back(measure); + } + } + }; + return scanGenerator; +} diff --git a/repository/packages/r/rplidar/rplidar/xmake.lua b/repository/packages/r/rplidar/rplidar/xmake.lua new file mode 100644 index 00000000..eda1d5e8 --- /dev/null +++ b/repository/packages/r/rplidar/rplidar/xmake.lua @@ -0,0 +1,8 @@ +add_requires("serial", "spdlog") +target("RPLidar") + set_kind("$(kind)") + add_packages("serial", "spdlog") + add_files("src/*.cpp") + add_headerfiles("include/(**.h)") + add_headerfiles("include/(**.hpp)") + add_includedirs("include", {public = true}) diff --git a/repository/packages/r/rplidar/xmake.lua b/repository/packages/r/rplidar/xmake.lua new file mode 100644 index 00000000..00b8b0e6 --- /dev/null +++ b/repository/packages/r/rplidar/xmake.lua @@ -0,0 +1,9 @@ +package("rplidar") + add_deps("serial", "spdlog") + set_sourcedir(path.join(os.scriptdir(), "rplidar")) + set_policy("package.install_always", true) + on_install(function (package) + local configs = {} + import("package.tools.xmake").install(package, configs) + end) +package_end() \ No newline at end of file