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 new file mode 100644 index 0000000..53e5fa4 --- /dev/null +++ b/software/ros_ws/src/sensors/include/m2m2_lidar/m2m2_lidar.hpp @@ -0,0 +1,71 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +/** + * @brief Main class for interfacing with the M2M2 Lidar sensor + * + * This class handles both the network communication and protocol parsing + * for the M2M2 Lidar. While this combines two responsibilities, they are + * tightly coupled for this specific sensor and the data flow between + * them is well-defined. + */ +class M2M2Lidar : public rclcpp::Node +{ +public: + /** + * @brief Configuration parameters for the M2M2 Lidar + * + * Groups all sensor-specific configuration parameters in one place, + * making it easier to modify settings and extend functionality + */ + struct SensorConfig + { + double scanFrequency; + double angularResolution; + double minRange; + double maxRange; + }; + /** + * @brief Constructor for the M2M2 Lidar node + * @param options ROS2 node options for configuration + */ + explicit M2M2Lidar( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + /** + * @brief Clean up + */ + ~M2M2Lidar(); + +private: + // 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); + + // ROS setup + void _initializePublishers(); + void _publishData(); + void _readSensorData(); + + std::string _sensorAddress; + uint16_t _sensorPort; + int _socket; + bool _isConnected; + SensorConfig _config; + + // ROS2 publishers + rclcpp::Publisher::SharedPtr _scanPublisher; + rclcpp::Publisher::SharedPtr _imuPublisher; + rclcpp::TimerBase::SharedPtr _readTimer; +}; \ No newline at end of file diff --git a/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/m2m2_lidar.hpp b/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/m2m2_lidar.hpp deleted file mode 100644 index 49b7369..0000000 --- a/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/m2m2_lidar.hpp +++ /dev/null @@ -1,100 +0,0 @@ -// File: include/sensors/m2m2_lidar/m2m2_lidar.hpp - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace sensors -{ - namespace lidar - { - namespace m2m2 - { - - /** - * @brief Configuration parameters for the M2M2 Lidar - * - * Groups all sensor-specific configuration parameters in one place, - * making it easier to modify settings and extend functionality - */ - struct SensorConfig - { - double scanFrequency; - double angularResolution; - double minRange; - double maxRange; - }; - - /** - * @brief Main class for interfacing with the M2M2 Lidar sensor - * - * This class handles both the network communication and protocol parsing - * for the M2M2 Lidar. While this combines two responsibilities, they are - * tightly coupled for this specific sensor and the data flow between - * them is well-defined. - */ - class Lidar : public rclcpp::Node - { - public: - /** - * @brief Constructor for the M2M2 Lidar node - * @param options ROS2 node options for configuration - */ - explicit Lidar( - const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - /** - * @brief Destructor ensures proper cleanup of resources - */ - ~Lidar(); - - /** - * @brief Initialize the lidar connection and start data processing - * @return true if initialization successful - */ - bool initialize(); - - private: - // Network communication methods - bool connectToSensor(); - bool sendCommand(const std::vector& command); - std::optional> receiveData(); - void disconnect(); - - // Protocol handling methods - std::vector createConfigCommand(const SensorConfig& config); - std::optional parseLaserScanData( - const std::vector& data); - std::optional parseImuData( - const std::vector& data); - - // ROS2 interface methods - void initializePublishers(); - void publishData(); - void readSensorData(); - - // Member variables - std::string _sensorAddress; - uint16_t _sensorPort; - int _socket; - bool _isConnected; - SensorConfig _config; - - // Protocol-related constants - static constexpr uint8_t PROTOCOL_HEADER = 0xAA; - static constexpr uint8_t PROTOCOL_FOOTER = 0x55; - - // ROS2 publishers - rclcpp::Publisher::SharedPtr _scanPublisher; - rclcpp::Publisher::SharedPtr _imuPublisher; - rclcpp::TimerBase::SharedPtr _readTimer; - }; - - } - } -} // namespace sensors::lidar::m2m2 \ No newline at end of file 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 4b3e793..d931446 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 @@ -1,274 +1,180 @@ -// File: src/m2m2_lidar/m2m2_lidar.cpp - -#include "sensors/m2m2_lidar/m2m2_lidar.hpp" +#include "m2m2_lidar/m2m2_lidar.hpp" #include #include #include #include -#include - -namespace +M2M2Lidar::M2M2Lidar(const rclcpp::NodeOptions& options) + : Node("m2m2_lidar", options) { - // Helper functions that don't need class access - [[maybe_unused]] bool validateChecksum([[maybe_unused]] const std::vector& data) + // Parameter setup + this->declare_parameter("sensor_ip", "192.168.1.100"); + this->declare_parameter("sensor_port", 2000); + this->declare_parameter("frame_id", "lidar_frame"); + this->declare_parameter("scan_topic", "scan"); + this->declare_parameter("imu_topic", "imu"); + + // TODO: Error handling (exceptions) + _sensorAddress = this->get_parameter("sensor_ip").as_string(); + _sensorPort = static_cast( + this->get_parameter("sensor_port").as_int()); + + // TODO: Replace with network library + struct sockaddr_in serverAddr; + serverAddr.sin_family = AF_INET; + serverAddr.sin_port = htons(_sensorPort); + + if (inet_pton(AF_INET, _sensorAddress.c_str(), &serverAddr.sin_addr) <= 0) { - // Implementation of checksum validation - return true; // Placeholder + RCLCPP_ERROR(this->get_logger(), "Invalid address"); + // TODO: Throw exception } - [[maybe_unused]] uint16_t calculateChecksum([[maybe_unused]] const std::vector& data) + _socket = socket(AF_INET, SOCK_STREAM, 0); + if (_socket < 0) { - // Implementation of checksum calculation - return 0; // Placeholder + RCLCPP_ERROR(this->get_logger(), "Failed to create socket"); + // TODO: Throw exception } -} // anonymous namespace - -namespace sensors -{ - namespace lidar + if (connect(_socket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) { - namespace m2m2 - { - - Lidar::Lidar(const rclcpp::NodeOptions& options) - : Node("m2m2_lidar", options), _socket(-1), _isConnected(false) - { - // Declare and get parameters - this->declare_parameter("sensor_ip", "192.168.1.100"); - this->declare_parameter("sensor_port", 2000); - this->declare_parameter("frame_id", "lidar_frame"); - this->declare_parameter("scan_topic", "scan"); - this->declare_parameter("imu_topic", "imu"); - - _sensorAddress = this->get_parameter("sensor_ip").as_string(); - _sensorPort = static_cast( - this->get_parameter("sensor_port").as_int()); - - initializePublishers(); - } - - Lidar::~Lidar() - { - disconnect(); - } - - bool Lidar::initialize() - { - if (!connectToSensor()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to connect to sensor"); - return false; - } - - // Create and send initial configuration - SensorConfig defaultConfig{ - .scanFrequency = 15.0, - .angularResolution = 0.25, - .minRange = 0.1, - .maxRange = 30.0}; - - auto configCommand = createConfigCommand(defaultConfig); - if (!sendCommand(configCommand)) - { - RCLCPP_ERROR(this->get_logger(), "Failed to configure sensor"); - return false; - } - - // Start the data reading timer - _readTimer = this->create_wall_timer( - std::chrono::milliseconds(100), - std::bind(&Lidar::readSensorData, this)); - - return true; - } - - void Lidar::initializePublishers() - { - const auto qos = rclcpp::QoS(10); - _scanPublisher = this->create_publisher( - this->get_parameter("scan_topic").as_string(), qos); - _imuPublisher = this->create_publisher( - this->get_parameter("imu_topic").as_string(), qos); - } - - bool Lidar::connectToSensor() - { - _socket = socket(AF_INET, SOCK_STREAM, 0); - if (_socket < 0) - { - RCLCPP_ERROR(this->get_logger(), "Failed to create socket"); - return false; - } - - struct sockaddr_in serverAddr; - serverAddr.sin_family = AF_INET; - serverAddr.sin_port = htons(_sensorPort); - - if (inet_pton(AF_INET, _sensorAddress.c_str(), &serverAddr.sin_addr) <= 0) - { - RCLCPP_ERROR(this->get_logger(), "Invalid address"); - close(_socket); - _socket = -1; - return false; - } - - if (connect(_socket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) - { - RCLCPP_ERROR(this->get_logger(), "Connection failed"); - close(_socket); - _socket = -1; - return false; - } - - _isConnected = true; - return true; - } - - void Lidar::disconnect() - { - if (_socket != -1) - { - close(_socket); - _socket = -1; - } - _isConnected = false; - } + RCLCPP_ERROR(this->get_logger(), "Connection failed"); + close(_socket); + // TODO: Throw exception + } - std::vector Lidar::createConfigCommand(const SensorConfig& config) - { - std::vector command; - command.push_back(PROTOCOL_HEADER); + // Create and send initial configuration + SensorConfig defaultConfig{ + .scanFrequency = 15.0, + .angularResolution = 0.25, + .minRange = 0.1, + .maxRange = 30.0}; + // TODO: Exception handling + _sendCommand(_createConfigCommand(defaultConfig)); + + // Start the data reading timer + _readTimer = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&M2M2Lidar::_readSensorData, this)); + _initializePublishers(); +} + +M2M2Lidar::~M2M2Lidar() +{ + close(_socket); +} - // Add command type (example: 0x01 for configuration) - command.push_back(0x01); +void M2M2Lidar::_initializePublishers() +{ + const auto qos = rclcpp::QoS(10); + _scanPublisher = this->create_publisher( + this->get_parameter("scan_topic").as_string(), qos); + _imuPublisher = this->create_publisher( + this->get_parameter("imu_topic").as_string(), qos); +} + +std::vector M2M2Lidar::_createConfigCommand(const SensorConfig& config) +{ + // Protocol-related constants + static constexpr uint8_t PROTOCOL_HEADER = 0xAA; + static constexpr uint8_t PROTOCOL_FOOTER = 0x55; - // Add configuration parameters - // Note: This is a simplified example - you'll need to match your actual protocol - command.push_back(static_cast(config.scanFrequency)); - command.push_back(static_cast(config.angularResolution * 100)); + std::vector command; + command.push_back(PROTOCOL_HEADER); - // Add footer - command.push_back(PROTOCOL_FOOTER); + // Add command type (example: 0x01 for configuration) + command.push_back(0x01); - return command; - } + // Add configuration parameters + // Note: This is a simplified example - you'll need to match your actual protocol + command.push_back(static_cast(config.scanFrequency)); + command.push_back(static_cast(config.angularResolution * 100)); - bool Lidar::sendCommand(const std::vector& command) - { - if (!_isConnected) - { - RCLCPP_ERROR(this->get_logger(), "Not connected to sensor"); - return false; - } + // Add footer + command.push_back(PROTOCOL_FOOTER); - ssize_t bytesSent = send(_socket, command.data(), command.size(), 0); - if (bytesSent != static_cast(command.size())) - { - RCLCPP_ERROR(this->get_logger(), "Failed to send complete command"); - return false; - } + return command; +} - return true; - } +void M2M2Lidar::_sendCommand(const std::vector& command) +{ + ssize_t bytesSent = send(_socket, command.data(), command.size(), 0); + if (bytesSent != static_cast(command.size())) + { + RCLCPP_ERROR(this->get_logger(), "Failed to send complete command"); + // TODO: Throw exception + } +} - void Lidar::readSensorData() - { - if (!_isConnected) - { - RCLCPP_ERROR_THROTTLE( - this->get_logger(), - *this->get_clock(), - 1000, // Throttle to once per second - "Not connected to sensor"); - return; - } +void M2M2Lidar::_readSensorData() +{ + // 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); - // 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); + if (bytesRead <= 0) + { + RCLCPP_ERROR(this->get_logger(), "Error reading from sensor"); + return; + } - if (bytesRead <= 0) - { - RCLCPP_ERROR(this->get_logger(), "Error reading from sensor"); - return; - } + // Resize buffer to actual data received + buffer.resize(bytesRead); - // Resize buffer to actual data received - buffer.resize(bytesRead); + // Note: These are placeholder implementations - we need to implement + // proper parsing based on m2m2 protocol + // TODO: Error handling + auto maybeScan = _parseLaserScanData(buffer); + if (maybeScan) + { + maybeScan->header.stamp = this->now(); + maybeScan->header.frame_id = + this->get_parameter("frame_id").as_string(); + _scanPublisher->publish(*maybeScan); + } - // Note: These are placeholder implementations - we need to implement - // proper parsing based on m2m2 protocol - auto maybeScan = parseLaserScanData(buffer); - if (maybeScan) - { - maybeScan->header.stamp = this->now(); - maybeScan->header.frame_id = - this->get_parameter("frame_id").as_string(); - _scanPublisher->publish(*maybeScan); - } + auto maybeImu = _parseImuData(buffer); + if (maybeImu) + { + maybeImu->header.stamp = this->now(); + maybeImu->header.frame_id = + this->get_parameter("frame_id").as_string(); + _imuPublisher->publish(*maybeImu); + } +} - auto maybeImu = parseImuData(buffer); - if (maybeImu) - { - maybeImu->header.stamp = this->now(); - maybeImu->header.frame_id = - 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{}; - // We also need to implement the parsing methods - std::optional Lidar::parseLaserScanData([[maybe_unused]] const std::vector& data) - { - sensor_msgs::msg::LaserScan scan; - // This is a placeholder implementation - // You'll need to implement actual parsing based on your sensor's protocol - scan.angle_min = -3.14159f; - scan.angle_max = 3.14159f; - scan.angle_increment = 0.01745f; - scan.time_increment = 0.0001f; - scan.scan_time = 0.1f; - scan.range_min = 0.1f; - scan.range_max = 30.0f; + // TODO: Implement - return scan; - } + return scan; +} - std::optional Lidar::parseImuData([[maybe_unused]] const std::vector& data) - { - sensor_msgs::msg::Imu imu; - // This is a placeholder implementation - // You'll need to implement actual parsing based on your sensor's protocol - imu.orientation_covariance[0] = -1; // Orientation not provided - imu.angular_velocity_covariance[0] = 0.1; - imu.linear_acceleration_covariance[0] = 0.1; +std::optional M2M2Lidar::_parseImuData([[maybe_unused]] const std::vector& data) +{ + sensor_msgs::msg::Imu imu{}; - return imu; - } + // TODO: Implement - } - } -} // namespace sensors::lidar::m2m2 + return imu; +} -// the main function int main(int argc, char** argv) { rclcpp::init(argc, argv); - - // Initialise the m2m2 lidar node - auto lidar_node = std::make_shared(); - - if (!lidar_node->initialize()) + try + { + auto node = std::make_shared(); + rclcpp::spin(node); + } + catch (const std::exception& e) { - RCLCPP_ERROR(lidar_node->get_logger(), "Failed to initialise m2m2 lidar"); + // TODO: Maybe get a logger with rclcpp::get_logger() and log error, but that should be handled internally to the node anyway return 1; } - - // spin the node - rclcpp::spin(lidar_node); - rclcpp::shutdown(); return 0; } \ No newline at end of file