diff --git a/software/ros_ws/src/sensors/CMakeLists.txt b/software/ros_ws/src/sensors/CMakeLists.txt index 6a428ee..69aba67 100644 --- a/software/ros_ws/src/sensors/CMakeLists.txt +++ b/software/ros_ws/src/sensors/CMakeLists.txt @@ -1,12 +1,7 @@ -# File: CMakeLists.txt - cmake_minimum_required(VERSION 3.8) project(sensors) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -27,6 +22,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(OpenSSL REQUIRED) # Include directories include_directories(include) @@ -34,8 +30,12 @@ include_directories(include) # Add the m2m2_lidar executable add_executable(m2m2_lidar src/m2m2_lidar/m2m2_lidar.cpp) +# add dependencies ament_target_dependencies(m2m2_lidar rclcpp sensor_msgs std_msgs) +# Link OpenSSL libraries +target_link_libraries(m2m2_lidar OpenSSL::SSL OpenSSL::Crypto) + # Install targets install(TARGETS m2m2_lidar DESTINATION lib/${PROJECT_NAME}) diff --git a/software/ros_ws/src/sensors/include/m2m2_lidar/m2m2_lidar.hpp b/software/ros_ws/src/sensors/include/m2m2_lidar/m2m2_lidar.hpp index 53e5fa4..6e3203f 100644 --- a/software/ros_ws/src/sensors/include/m2m2_lidar/m2m2_lidar.hpp +++ b/software/ros_ws/src/sensors/include/m2m2_lidar/m2m2_lidar.hpp @@ -1,5 +1,6 @@ #pragma once +#include // JSON parsing #include #include #include @@ -44,14 +45,24 @@ class M2M2Lidar : public rclcpp::Node ~M2M2Lidar(); private: + int _requestId; + static constexpr const char* REQUEST_DELIM = "\r\n\r\n"; + + std::vector _decodeBase64(const std::string& encoded); + + std::vector> _decodeLaserPoints(const std::string& base64_encoded); + + bool _sendJsonRequest(const std::string& command, const nlohmann::json& args = nullptr); + nlohmann::json _receiveJsonResponse(); + // Network communication void _sendCommand(const std::vector& command); std::optional> _receiveData(); // Protocol handling std::vector _createConfigCommand(const SensorConfig& config); - std::optional _parseLaserScanData(const std::vector& data); - std::optional _parseImuData(const std::vector& data); + // std::optional _parseLaserScanData(const std::vector& data); + // std::optional _parseImuData(const std::vector& data); // ROS setup void _initializePublishers(); diff --git a/software/ros_ws/src/sensors/src/m2m2_lidar/m2m2_lidar.cpp b/software/ros_ws/src/sensors/src/m2m2_lidar/m2m2_lidar.cpp index 0c66291..d256401 100644 --- a/software/ros_ws/src/sensors/src/m2m2_lidar/m2m2_lidar.cpp +++ b/software/ros_ws/src/sensors/src/m2m2_lidar/m2m2_lidar.cpp @@ -39,9 +39,13 @@ #include #include +#include // for Base64 decoding +#include // also for Base64 decoding #include #include +#include // JSON parsing + M2M2Lidar::M2M2Lidar(const rclcpp::NodeOptions& options) : Node("m2m2_lidar", options) { @@ -111,6 +115,168 @@ void M2M2Lidar::_initializePublishers() this->get_parameter("imu_topic").as_string(), qos); } +/** + * @brief Decodes a base64-encoded string into a byte sequence. + * + */ +std::vector M2M2Lidar::_decodeBase64(const std::string& encoded) +{ + BIO *bio, *b64; + std::vector decoded; + + b64 = BIO_new(BIO_f_base64()); + bio = BIO_new_mem_buf(encoded.c_str(), encoded.length()); + bio = BIO_push(b64, bio); + + BIO_set_flags(bio, BIO_FLAGS_BASE64_NO_NL); + + char buffer[1024]; + int len; + while ((len = BIO_read(bio, buffer, sizeof(buffer))) > 0) + { + decoded.insert(decoded.end(), buffer, buffer + len); + } + + BIO_free_all(bio); + return decoded; +} + +std::vector> M2M2Lidar::_decodeLaserPoints(const std::string& base64_encoded) +{ + std::vector> points; + + std::vector decoded = _decodeBase64(base64_encoded); + + if (decoded.size() < 9 || + decoded[0] != 'R' || + decoded[1] != 'L' || + decoded[2] != 'E') + { + RCLCPP_ERROR(this->get_logger(), "Invalid RLE header"); + return points; + } + + // RLE decompression + std::vector decompressed; + uint8_t sentinel1 = decoded[3]; + uint8_t sentinel2 = decoded[4]; + + size_t pos = 9; + while (pos < decoded.size()) + { + uint8_t b = decoded[pos]; + + if (b == sentinel1) + { + if (pos + 2 < decoded.size() && decoded[pos + 1] == 0 && decoded[pos + 2] == sentinel2) + { + std::swap(sentinel1, sentinel2); + pos += 2; + } + else if (pos + 2 < decoded.size()) + { + uint8_t repeat_val = decoded[pos + 2]; + uint8_t repeat_count = decoded[pos + 1]; + for (uint8_t i = 0; 1 < repeat_count; i++) + { + decompressed.push_back(repeat_val); + } + pos += 2; + } + } + else + { + decompressed.push_back(b); + } + ++pos; + } + + // Parse points + for (size_t i = 0; i < decompressed.size(); i += 12) + { + if (i + 12 > decompressed.size()) + break; + + float distance, angle; + memcpy(&distance, &decompressed[i], sizeof(float)); + memcpy(&angle, &decompressed[i + 4], sizeof(float)); + + bool valid = distance != 100000.0; // TODO fix magic number + points.emplace_back(distance, angle, valid); + } + return points; +} + +bool M2M2Lidar::_sendJsonRequest(const std::string& command, const nlohmann::json& args) +{ + nlohmann::json request = { + {"command", command}, + {"request_id", _requestId++}}; + + if (!args.is_null()) + { + request["args"] = args; + } + + std::string json_str = request.dump(); + std::vector data(json_str.begin(), json_str.end()); + + // append the delimiters + for (int i = 0; i < 3; ++i) + { + data.push_back('\r'); + data.push_back('\n'); + } + + ssize_t sent = send(_socket, data.data(), data.size(), 0); + if (sent <= 0) + { + RCLCPP_ERROR(this->get_logger(), "Send failed: %s", strerror(errno)); + return false; + } + return true; +} + +nlohmann::json M2M2Lidar::_receiveJsonResponse() +{ + std::vector buffer(4096); + std::vector received; + bool found_delim = false; + ssize_t bytes = 0; + + while (!found_delim) + { + ssize_t bytes = recv(_socket, buffer.data(), buffer.size(), 0); + if (bytes <= 0) + { + RCLCPP_ERROR(this->get_logger(), "Receive error: %s", strerror(errno)); + return {}; + } + } + + received.insert(received.end(), buffer.begin(), buffer.begin() + bytes); + + if (received.size() >= 4 && + received[received.size() - 4] == '\r' && + received[received.size() - 3] == '\n' && + received[received.size() - 2] == '\r' && + received[received.size() - 1] == '\n') + { + found_delim = true; + } + + std::string response_str(received.begin(), received.end()); + try + { + return nlohmann::json::parse(response_str); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(this->get_logger(), "Failed to parse JSON response: %s", e.what()); + return {}; + } +} + /** * @brief Creates a configuration command packet for the M2M2 LIDAR sensor. * @@ -155,6 +321,51 @@ void M2M2Lidar::_sendCommand(const std::vector& command) void M2M2Lidar::_readSensorData() { + if (!_sendJsonRequest("getlaserscan")) + { + RCLCPP_ERROR(this->get_logger(), "Failed to send getlaserscan command"); + return; + } + + auto response = _receiveJsonResponse(); + if (response.empty() || + response["result"].is_null() || + !response["result"].contains("laser_points")) + + { + RCLCPP_ERROR(this->get_logger(), "Failed to receive valid laser scan response"); + return; + } + + std::string base64_points = response["result"]["laser_points"]; + auto points = _decodeLaserPoints(base64_points); + + // populate the LaserScan message + sensor_msgs::msg::LaserScan scan; + scan.header.stamp = this->now(); + scan.header.frame_id = this->get_parameter("frame_id").as_string(); + + if (!points.empty()) + { + scan.angle_min = std::get<0>(points.front()); + scan.angle_max = std::get<0>(points.back()); + scan.angle_increment = (scan.angle_max - scan.angle_min) / (points.size() - 1); + scan.time_increment = 0.0; + scan.scan_time = 0.0; // TODO: set this based on sensor specs + scan.range_min = 0.0; // TODO: set this based on sensor specs + scan.range_max = 40.0; + + scan.ranges.reserve(points.size()); + for (const auto& [angle, distance, valid] : points) + { + scan.ranges.push_back(valid ? distance : 0.0); + } + } + + _scanPublisher->publish(scan); + + //******************************************************* + // Buffer for receiving data std::vector buffer(1024); // Adjust size based on your sensor's needs ssize_t bytesRead = recv(_socket, buffer.data(), buffer.size(), 0); @@ -171,6 +382,7 @@ void M2M2Lidar::_readSensorData() // Note: These are placeholder implementations - we need to implement // proper parsing based on m2m2 protocol // TODO: Error handling + /* auto maybeScan = _parseLaserScanData(buffer); if (maybeScan) { @@ -188,24 +400,29 @@ void M2M2Lidar::_readSensorData() this->get_parameter("frame_id").as_string(); _imuPublisher->publish(*maybeImu); } -} + */ -std::optional M2M2Lidar::_parseLaserScanData([[maybe_unused]] const std::vector& data) -{ - sensor_msgs::msg::LaserScan scan{}; + /* + *replaced now + std::optional M2M2Lidar::_parseLaserScanData([[maybe_unused]] const std::vector& data) + { + sensor_msgs::msg::LaserScan scan{}; - // TODO: Implement + // TODO: Implement - return scan; -} + return scan; + } -std::optional M2M2Lidar::_parseImuData([[maybe_unused]] const std::vector& data) -{ - sensor_msgs::msg::Imu imu{}; + std::optional M2M2Lidar::_parseImuData([[maybe_unused]] const std::vector& data) + { + sensor_msgs::msg::Imu imu{}; - // TODO: Implement + // TODO: Implement + + return imu; + } - return imu; + */ } int main(int argc, char** argv)