From 6cfdee8fbd4fe7fc9e85f606212f014ad4a0fba7 Mon Sep 17 00:00:00 2001 From: DingoOz Date: Wed, 18 Dec 2024 20:56:56 +1000 Subject: [PATCH] improved namespace and class hierarchy. Should build with warnings, does not yet do anything useful --- software/ros_ws/src/sensors/CMakeLists.txt | 58 ++-- .../sensors/m2m2_lidar/communication.hpp | 55 ---- .../include/sensors/m2m2_lidar/m2m2_lidar.hpp | 100 +++++++ .../include/sensors/m2m2_lidar/protocol.hpp | 53 ---- .../sensors/src/m2m2_lidar/communication.cpp | 76 ----- .../src/sensors/src/m2m2_lidar/m2m2_lidar.cpp | 276 ++++++++++++++++++ .../src/sensors/src/m2m2_lidar/main.cpp | 89 ------ .../src/sensors/src/m2m2_lidar/protocol.cpp | 60 ---- 8 files changed, 396 insertions(+), 371 deletions(-) delete mode 100644 software/ros_ws/src/sensors/include/sensors/m2m2_lidar/communication.hpp create mode 100644 software/ros_ws/src/sensors/include/sensors/m2m2_lidar/m2m2_lidar.hpp delete mode 100644 software/ros_ws/src/sensors/include/sensors/m2m2_lidar/protocol.hpp delete mode 100644 software/ros_ws/src/sensors/src/m2m2_lidar/communication.cpp create mode 100644 software/ros_ws/src/sensors/src/m2m2_lidar/m2m2_lidar.cpp delete mode 100644 software/ros_ws/src/sensors/src/m2m2_lidar/main.cpp delete mode 100644 software/ros_ws/src/sensors/src/m2m2_lidar/protocol.cpp diff --git a/software/ros_ws/src/sensors/CMakeLists.txt b/software/ros_ws/src/sensors/CMakeLists.txt index c5d7847..6a428ee 100644 --- a/software/ros_ws/src/sensors/CMakeLists.txt +++ b/software/ros_ws/src/sensors/CMakeLists.txt @@ -1,3 +1,5 @@ +# File: CMakeLists.txt + cmake_minimum_required(VERSION 3.8) project(sensors) @@ -10,59 +12,39 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # Add -Werror flag for release builds + set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Werror") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Set the C++ standard +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) -#incldue dirs +# Include directories include_directories(include) # Add the m2m2_lidar executable -add_executable(m2m2_lidar - src/m2m2_lidar/main.cpp - src/m2m2_lidar/communication.cpp - src/m2m2_lidar/protocol.cpp -) - -#add_executable(m2m2_lidar src/m2m2_lidar.cpp) -#target_include_directories(m2m2_lidar PUBLIC -# $ -# $) -#target_compile_features(m2m2_lidar PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - - -ament_target_dependencies( - m2m2_lidar - "rclcpp" - "sensor_msgs" - "std_msgs" -) +add_executable(m2m2_lidar src/m2m2_lidar/m2m2_lidar.cpp) -#install targets -install(TARGETS m2m2_lidar - DESTINATION lib/${PROJECT_NAME}) +ament_target_dependencies(m2m2_lidar rclcpp sensor_msgs std_msgs) -#install include directory -install(DIRECTORY include/ - DESTINATION include/ -) +# Install targets +install(TARGETS m2m2_lidar DESTINATION lib/${PROJECT_NAME}) -#Install launch files -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} -) +# Install include directory +install(DIRECTORY include/ DESTINATION include/) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/communication.hpp b/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/communication.hpp deleted file mode 100644 index 6a3570b..0000000 --- a/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/communication.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace sensors -{ - class Communication - { - public: - /** - * @brief Constructor for the Communication class - * @param address IP address of the M2M2 Lidar sensor - * @param port Port number for communication - */ - explicit Communication(const std::string& address, uint16_t port); - - /** - * @brief Destructor ensuring proper cleanup of resources - */ - ~Communication(); - - /** - * @brief Initialize the communication with the sensor - * @return true if initialization successful, false otherwise - */ - bool initialize(); - - /** - * @brief Send a command to the sensor - * @param command Command buffer to send - * @param length Length of the command - * @return true if send successful, false otherwise - */ - bool sendCommand(const uint8_t* command, size_t length); - - /** - * @brief Receive data from the sensor - * @param buffer Buffer to store received data - * @param maxLength Maximum length of data to receive - * @return Number of bytes received, or -1 on error - */ - ssize_t receiveData(uint8_t* buffer, size_t maxLength); - - private: - std::string _address; - uint16_t _port; - int _socket; -}; - -} // namespace sensors 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 new file mode 100644 index 0000000..49b7369 --- /dev/null +++ b/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/m2m2_lidar.hpp @@ -0,0 +1,100 @@ +// 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/include/sensors/m2m2_lidar/protocol.hpp b/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/protocol.hpp deleted file mode 100644 index 07b4333..0000000 --- a/software/ros_ws/src/sensors/include/sensors/m2m2_lidar/protocol.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -namespace sensors -{ - class Protocol - { - public: - /** - * @brief Constructor for the Protocol class - */ - Protocol(); - - /** - * @brief Destructor - */ - ~Protocol(); - - /** - * @brief Parse raw sensor data into LaserScan message - * @param data Raw data buffer from sensor - * @param length Length of data buffer - * @return Parsed LaserScan message - */ - sensor_msgs::msg::LaserScan parseLaserScanData(const uint8_t* data, size_t length); - - /** - * @brief Parse raw sensor data into IMU message - * @param data Raw data buffer from sensor - * @param length Length of data buffer - * @return Parsed IMU message - */ - sensor_msgs::msg::Imu parseImuData(const uint8_t* data, size_t length); - - /** - * @brief Create command buffer for sensor configuration - * @param buffer Output buffer for command - * @param maxLength Maximum length of buffer - * @return Length of command written to buffer - */ - size_t createConfigCommand(uint8_t* buffer, size_t maxLength); - - private: - // Internal protocol-specific constants - static constexpr uint8_t PROTOCOL_HEADER = 0xAA; - static constexpr uint8_t PROTOCOL_FOOTER = 0x55; -}; - -} // namespace sensors diff --git a/software/ros_ws/src/sensors/src/m2m2_lidar/communication.cpp b/software/ros_ws/src/sensors/src/m2m2_lidar/communication.cpp deleted file mode 100644 index 6be1bf5..0000000 --- a/software/ros_ws/src/sensors/src/m2m2_lidar/communication.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include "sensors/m2m2_lidar/communication.hpp" -#include -#include -#include -#include -#include - -namespace sensors -{ - -Communication::Communication(const std::string& address, uint16_t port) - : _address(address) - , _port(port) - , _socket(-1) -{ -} - -Communication::~Communication() -{ - if (_socket != -1) - { - close(_socket); - } -} - -bool Communication::initialize() -{ - _socket = socket(AF_INET, SOCK_STREAM, 0); - if (_socket < 0) - { - return false; - } - - struct sockaddr_in serverAddr; - serverAddr.sin_family = AF_INET; - serverAddr.sin_port = htons(_port); - - if (inet_pton(AF_INET, _address.c_str(), &serverAddr.sin_addr) <= 0) - { - close(_socket); - _socket = -1; - return false; - } - - if (connect(_socket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) - { - close(_socket); - _socket = -1; - return false; - } - - return true; -} - -bool Communication::sendCommand(const uint8_t* command, size_t length) -{ - if (_socket == -1) - { - return false; - } - - ssize_t bytesSent = send(_socket, command, length, 0); - return bytesSent == static_cast(length); -} - -ssize_t Communication::receiveData(uint8_t* buffer, size_t maxLength) -{ - if (_socket == -1) - { - return -1; - } - - return recv(_socket, buffer, maxLength, 0); -} - -} // namespace sensors 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 new file mode 100644 index 0000000..c705760 --- /dev/null +++ b/software/ros_ws/src/sensors/src/m2m2_lidar/m2m2_lidar.cpp @@ -0,0 +1,276 @@ +// File: src/m2m2_lidar/m2m2_lidar.cpp + +#include "sensors/m2m2_lidar/m2m2_lidar.hpp" + +#include +#include +#include +#include + +#include + +namespace +{ + // Helper functions that don't need class access + bool validateChecksum(const std::vector& data) + { + // Implementation of checksum validation + return true; // Placeholder + } + + uint16_t calculateChecksum(const std::vector& data) + { + // Implementation of checksum calculation + return 0; // Placeholder + } +} // anonymous namespace + +namespace sensors +{ + namespace lidar + { + 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; + } + + std::vector Lidar::createConfigCommand(const SensorConfig& config) + { + std::vector command; + command.push_back(PROTOCOL_HEADER); + + // Add command type (example: 0x01 for configuration) + command.push_back(0x01); + + // 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)); + + // Add footer + command.push_back(PROTOCOL_FOOTER); + + return command; + } + + bool Lidar::sendCommand(const std::vector& command) + { + if (!_isConnected) + { + RCLCPP_ERROR(this->get_logger(), "Not connected to sensor"); + return false; + } + + 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 true; + } + + 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; + } + + // 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; + } + + // Resize buffer to actual data received + buffer.resize(bytesRead); + + // 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); + } + } + + // We also need to implement the parsing methods + std::optional Lidar::parseLaserScanData( + 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; + + return scan; + } + + std::optional Lidar::parseImuData( + 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; + + return imu; + } + + } + } +} // namespace sensors::lidar::m2m2 + +// 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()) + { + RCLCPP_ERROR(lidar_node->get_logger(), "Failed to initialise m2m2 lidar"); + return 1; + } + + // spin the node + rclcpp::spin(lidar_node); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/software/ros_ws/src/sensors/src/m2m2_lidar/main.cpp b/software/ros_ws/src/sensors/src/m2m2_lidar/main.cpp deleted file mode 100644 index 805baee..0000000 --- a/software/ros_ws/src/sensors/src/m2m2_lidar/main.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include -#include -#include "sensors/m2m2_lidar/communication.hpp" -#include "sensors/m2m2_lidar/protocol.hpp" - -namespace sensors -{ - class M2M2Lidar : public rclcpp::Node - { - public: - /** - * @brief Constructor for the M2M2Lidar node - */ - M2M2Lidar() - : Node("m2m2_lidar") - { - // Declare 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"); - - // Get parameters - const auto sensorIp = this->get_parameter("sensor_ip").as_string(); - const auto sensorPort = static_cast(this->get_parameter("sensor_port").as_int()); - const auto frameId = this->get_parameter("frame_id").as_string(); - const auto scanTopic = this->get_parameter("scan_topic").as_string(); - const auto imuTopic = this->get_parameter("imu_topic").as_string(); - - // Initialize communication and protocol handlers - _communication = std::make_unique(sensorIp, sensorPort); - _protocol = std::make_unique(); - - // Create publishers - _scanPublisher = this->create_publisher( - scanTopic, rclcpp::QoS(10)); - _imuPublisher = this->create_publisher( - imuTopic, rclcpp::QoS(10)); - - // Create timer for periodic data reading - _timer = this->create_wall_timer( - std::chrono::milliseconds(100), - std::bind(&M2M2Lidar::readSensorData, this)); - - RCLCPP_INFO(this->get_logger(), "M2M2 Lidar node initialized"); - } - - private: - /** - * @brief Timer callback for reading sensor data - */ - void readSensorData() - { - static uint8_t buffer[4096]; - const auto bytesRead = _communication->receiveData(buffer, sizeof(buffer)); - - if (bytesRead > 0) - { - // Process laser scan data - auto scanMsg = _protocol->parseLaserScanData(buffer, bytesRead); - scanMsg.header.stamp = this->now(); - scanMsg.header.frame_id = this->get_parameter("frame_id").as_string(); - _scanPublisher->publish(scanMsg); - - // Process IMU data - auto imuMsg = _protocol->parseImuData(buffer, bytesRead); - imuMsg.header.stamp = this->now(); - imuMsg.header.frame_id = this->get_parameter("frame_id").as_string(); - _imuPublisher->publish(imuMsg); - } - } - - std::unique_ptr _communication; - std::unique_ptr _protocol; - rclcpp::Publisher::SharedPtr _scanPublisher; - rclcpp::Publisher::SharedPtr _imuPublisher; - rclcpp::TimerBase::SharedPtr _timer; - }; -} // namespace sensors - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/software/ros_ws/src/sensors/src/m2m2_lidar/protocol.cpp b/software/ros_ws/src/sensors/src/m2m2_lidar/protocol.cpp deleted file mode 100644 index d664bc6..0000000 --- a/software/ros_ws/src/sensors/src/m2m2_lidar/protocol.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "sensors/m2m2_lidar/protocol.hpp" - -namespace sensors -{ - -Protocol::Protocol() -{ - // Initialize any protocol-specific state here -} - -Protocol::~Protocol() -{ - // Clean up any protocol-specific resources here -} - -sensor_msgs::msg::LaserScan Protocol::parseLaserScanData(const uint8_t* data, size_t length) -{ - sensor_msgs::msg::LaserScan scan; - // TODO: Implement actual parsing based on M2M2 protocol specification - // This is a placeholder implementation - 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; - - return scan; -} - -sensor_msgs::msg::Imu Protocol::parseImuData(const uint8_t* data, size_t length) -{ - sensor_msgs::msg::Imu imu; - // TODO: Implement actual parsing based on M2M2 protocol specification - // This is a placeholder implementation - imu.orientation_covariance[0] = -1; // Orientation not provided - imu.angular_velocity_covariance[0] = 0.1; - imu.linear_acceleration_covariance[0] = 0.1; - - return imu; -} - -size_t Protocol::createConfigCommand(uint8_t* buffer, size_t maxLength) -{ - if (maxLength < 3) - { - return 0; - } - - // TODO: Implement actual command creation based on M2M2 protocol specification - // This is a placeholder implementation - buffer[0] = PROTOCOL_HEADER; - buffer[1] = 0x01; // Example command - buffer[2] = PROTOCOL_FOOTER; - - return 3; -} - -} // namespace sensors