diff --git a/src/Bringup/launch/science.launch.py b/src/Bringup/launch/science.launch.py index 06d59153..f35823de 100644 --- a/src/Bringup/launch/science.launch.py +++ b/src/Bringup/launch/science.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): joystick_control_dir = get_package_share_directory("joystick_control") - joy_parameters_file = os.path.join(joystick_control_dir, "pxn.yaml") + joy_parameters_file = os.path.join(joystick_control_dir, "3dpro.yaml") science_drill_control = Node( package="joystick_control", @@ -40,6 +40,7 @@ def generate_launch_description(): plugin="ros_phoenix::TalonSRX", name="elevator", parameters=[ + {"interface": "can1"}, {"id": 20}, {"max_voltage": 24.0}, {"brake_mode": True}, @@ -51,6 +52,7 @@ def generate_launch_description(): plugin="ros_phoenix::TalonSRX", name="drill", parameters=[ + {"interface": "can1"}, {"id": 21}, {"max_voltage": 24.0}, {"brake_mode": True}, @@ -61,28 +63,22 @@ def generate_launch_description(): output="screen", ) - esp_serial_bridge = Node( + science_sensors = Node( package="science_sensors", - executable="esp_serial_bridge", - name="esp_serial_bridge", + executable="science_sensors", + name="science_sensors", + parameters=[{"interface": "can1"}], ) panoramic = Node( - package="science_sensors", + package="science_python", executable="panoramic", name="panoramic", ) - polarimeter = Node( - package="science_sensors", - executable="polarimeter", - name="polarimeter", - ) - ld = LaunchDescription() ld.add_action(science_drill_control) ld.add_action(talon_container) - ld.add_action(esp_serial_bridge) - ld.add_action(polarimeter) + ld.add_action(science_sensors) ld.add_action(panoramic) return ld diff --git a/src/HW-Devices/science_python/package.xml b/src/HW-Devices/science_python/package.xml new file mode 100644 index 00000000..e93dfd1e --- /dev/null +++ b/src/HW-Devices/science_python/package.xml @@ -0,0 +1,22 @@ + + + + science_python + 0.0.0 + Nodes for science sensors + aydan + TODO: License declaration + + rclpy + interfaces + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/src/HW-Devices/science_sensors/science_sensors/__init__.py b/src/HW-Devices/science_python/resource/science_python similarity index 100% rename from src/HW-Devices/science_sensors/science_sensors/__init__.py rename to src/HW-Devices/science_python/resource/science_python diff --git a/src/HW-Devices/science_python/science_python/__init__.py b/src/HW-Devices/science_python/science_python/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/HW-Devices/science_sensors/science_sensors/make_graph.py b/src/HW-Devices/science_python/science_python/make_graph.py similarity index 100% rename from src/HW-Devices/science_sensors/science_sensors/make_graph.py rename to src/HW-Devices/science_python/science_python/make_graph.py diff --git a/src/HW-Devices/science_sensors/science_sensors/panoramic.py b/src/HW-Devices/science_python/science_python/panoramic.py similarity index 100% rename from src/HW-Devices/science_sensors/science_sensors/panoramic.py rename to src/HW-Devices/science_python/science_python/panoramic.py diff --git a/src/HW-Devices/science_python/setup.cfg b/src/HW-Devices/science_python/setup.cfg new file mode 100644 index 00000000..9245e20e --- /dev/null +++ b/src/HW-Devices/science_python/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/science_python +[install] +install_scripts=$base/lib/science_python \ No newline at end of file diff --git a/src/HW-Devices/science_python/setup.py b/src/HW-Devices/science_python/setup.py new file mode 100644 index 00000000..9702f4d7 --- /dev/null +++ b/src/HW-Devices/science_python/setup.py @@ -0,0 +1,33 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = "science_python" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages( + include=["science_python", "science_python.*"], exclude=["test"] + ), + include_package_data=True, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ( + os.path.join("share", package_name, "launch"), + glob(os.path.join("launch", "*launch.[pxy][yma]*")), + ), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="aydan", + maintainer_email="aj01cars@outlook.com", + description="Nodes for science sensors", + license="TODO: License declaration", + entry_points={ + "console_scripts": [ + "panoramic = science_python.panoramic:main", + ], + }, +) diff --git a/src/HW-Devices/science_sensors/CMakeLists.txt b/src/HW-Devices/science_sensors/CMakeLists.txt new file mode 100644 index 00000000..856b4ab0 --- /dev/null +++ b/src/HW-Devices/science_sensors/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.5) +project(science_sensors LANGUAGES CXX) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(interfaces REQUIRED) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/../ros_odrive/odrive_base/include + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +add_executable(${PROJECT_NAME} + "../ros_odrive/odrive_base/src/epoll_event_loop.cpp" + "../ros_odrive/odrive_base/src/socket_can.cpp" + "src/science_node.cpp" + "src/main.cpp" +) +ament_target_dependencies(${PROJECT_NAME} + "rclcpp" + "interfaces" +) + +# Install Components +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +# Install headers +install( + DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories( + include/ +) +ament_export_libraries( + ${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/src/HW-Devices/science_sensors/include/science_node.hpp b/src/HW-Devices/science_sensors/include/science_node.hpp new file mode 100644 index 00000000..a4c80504 --- /dev/null +++ b/src/HW-Devices/science_sensors/include/science_node.hpp @@ -0,0 +1,65 @@ +#ifndef SCIENCE_SENSORS_HPP +#define SCIENCE_SENSORS_HPP + +#include +#include +#include +#include + +#include "epoll_event_loop.hpp" +#include "interfaces/msg/dht22.hpp" +#include "interfaces/msg/polarimeter_sweep.hpp" +#include "interfaces/msg/science_adc.hpp" +#include "interfaces/msg/science_motor.hpp" +#include "interfaces/msg/science_servo.hpp" +#include "interfaces/srv/run_polarimeter.hpp" +#include "socket_can.hpp" + +#define SCAN_STEPS 48 + +class ScienceNode : public rclcpp::Node { +public: + ScienceNode(const std::string name, + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + bool init(EpollEventLoop *event_loop); + void deinit(); + +private: + void recv_callback(const can_frame &frame); + void motor_callback(); + void servo_callback(); + void polar_callback( + const std::shared_ptr request, + std::shared_ptr response); + void request_polar(); + + inline bool verify_length(const std::string &name, uint8_t expected, + uint8_t length); + + uint16_t node_id_; + SocketCanIntf can_intf_ = SocketCanIntf(); + + rclcpp::Publisher::SharedPtr adc_pub_; + rclcpp::Publisher::SharedPtr temp_pub_; + rclcpp::Publisher::SharedPtr co2_pub_; + rclcpp::Publisher::SharedPtr polar_pub_; + + EpollEvent motor_evt_; + std::mutex motor_mutex_; + interfaces::msg::ScienceMotor motor_msg_ = interfaces::msg::ScienceMotor(); + rclcpp::Subscription::SharedPtr motor_sub_; + + EpollEvent servo_evt_; + std::mutex servo_mutex_; + interfaces::msg::ScienceServo servo_msg_ = interfaces::msg::ScienceServo(); + rclcpp::Subscription::SharedPtr servo_sub_; + + EpollEvent req_polar_evt_; + std::condition_variable polar_cond_; + std::mutex polar_mutex_; + rclcpp::Service::SharedPtr polar_srv_; + uint16_t polar_points_[SCAN_STEPS]; + uint8_t polar_index_; +}; + +#endif // SCIENCE_SENORS_HPP \ No newline at end of file diff --git a/src/HW-Devices/science_sensors/launch/can_module_reader.launch.py b/src/HW-Devices/science_sensors/launch/can_module_reader.launch.py deleted file mode 100644 index 60076f9b..00000000 --- a/src/HW-Devices/science_sensors/launch/can_module_reader.launch.py +++ /dev/null @@ -1,14 +0,0 @@ -import launch -import launch_ros.actions - - -def generate_launch_description(): - return launch.LaunchDescription( - [ - launch_ros.actions.Node( - package="science_sensors", - executable="can_module_sensor", - name="gas_sensor", - ), - ] - ) diff --git a/src/HW-Devices/science_sensors/launch/gas_sensor.launch.py b/src/HW-Devices/science_sensors/launch/gas_sensor.launch.py deleted file mode 100644 index e16a80f7..00000000 --- a/src/HW-Devices/science_sensors/launch/gas_sensor.launch.py +++ /dev/null @@ -1,17 +0,0 @@ -import launch -import launch_ros.actions - - -def generate_launch_description(): - return launch.LaunchDescription( - [ - launch_ros.actions.Node( - package="science_sensors", - executable="gas_sensor", - name="gas_sensor", - parameters=[ - {"sea_level_pressure_hpa": 1013.25}, - ], - ), - ] - ) diff --git a/src/HW-Devices/science_sensors/launch/gpio.launch.py b/src/HW-Devices/science_sensors/launch/gpio.launch.py deleted file mode 100644 index c70aa492..00000000 --- a/src/HW-Devices/science_sensors/launch/gpio.launch.py +++ /dev/null @@ -1,39 +0,0 @@ -import launch -import launch_ros.actions - - -def generate_launch_description(): - return launch.LaunchDescription( - [ - launch_ros.actions.Node( - package="science_sensors", - executable="pi_gpio_controller", - name="microscope_light", - parameters=[ - {"service_name": "/microscope_light"}, - {"gpio_pins": [11, 6]}, - ], - ), - launch_ros.actions.Node( - package="science_sensors", - executable="pi_gpio_controller", - name="raman_light", - parameters=[ - {"service_name": "/raman_light"}, - {"gpio_pins": [7]}, - ], - ), - launch_ros.actions.Node( - package="science_sensors", - executable="pi_gpio_reader", - name="gpio_reader_node", - parameters=[ - {"gpio_pins": [12]}, - {"interval": 1.0}, - ], - remappings=[ - ("/gpio/12", "/science/ground"), - ], - ), - ] - ) diff --git a/src/HW-Devices/science_sensors/launch/panoramic.launch.py b/src/HW-Devices/science_sensors/launch/panoramic.launch.py deleted file mode 100644 index c8da4489..00000000 --- a/src/HW-Devices/science_sensors/launch/panoramic.launch.py +++ /dev/null @@ -1,14 +0,0 @@ -import launch -import launch_ros.actions - - -def generate_launch_description(): - return launch.LaunchDescription( - [ - launch_ros.actions.Node( - package="science_sensors", - executable="panoramic", - name="panoramic", - ), - ] - ) diff --git a/src/HW-Devices/science_sensors/launch/talon.launch.py b/src/HW-Devices/science_sensors/launch/talon.launch.py deleted file mode 100644 index d6138820..00000000 --- a/src/HW-Devices/science_sensors/launch/talon.launch.py +++ /dev/null @@ -1,45 +0,0 @@ -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def generate_launch_description(): - """Generate launch description with multiple components.""" - container = ComposableNodeContainer( - name="PhoenixContainerScience", - namespace="", - package="ros_phoenix", - executable="phoenix_container", - parameters=[{"interface": "can0"}], - composable_node_descriptions=[ - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="platform", - parameters=[ - {"id": 7}, - {"P": 2.0}, - {"I": 0.0}, - {"D": 0.0}, - {"max_voltage": 24.0}, - {"brake_mode": True}, - ], - ), - ComposableNode( - package="ros_phoenix", - plugin="ros_phoenix::TalonSRX", - name="drill", - parameters=[ - {"id": 8}, - {"max_voltage": 24.0}, - {"brake_mode": True}, - ], - ), - ], - ) - - return launch.LaunchDescription( - [ - container, - ] - ) diff --git a/src/HW-Devices/science_sensors/package.xml b/src/HW-Devices/science_sensors/package.xml index 37b34fbb..84e496b6 100644 --- a/src/HW-Devices/science_sensors/package.xml +++ b/src/HW-Devices/science_sensors/package.xml @@ -7,10 +7,13 @@ aydan TODO: License declaration + ament_cmake + ament_cmake_python + rclpy + rclcpp interfaces std_msgs - python3-serial ament_copyright ament_flake8 @@ -18,6 +21,6 @@ python3-pytest - ament_python + ament_cmake diff --git a/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py b/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py deleted file mode 100644 index bb5e7e72..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py +++ /dev/null @@ -1,285 +0,0 @@ -#!/usr/bin/env python3 -import os -import struct - -import rclpy -from rclpy.node import Node - -from interfaces.msg import EspSensorReadings, PolarimeterSweep, PwmCommand - -try: - import serial - from serial import SerialException -except ImportError: # pragma: no cover - runtime dependency check - serial = None - SerialException = Exception - - -class EspSerialBridge(Node): - _PWM_STRUCT = struct.Struct(" str: - port = (port_value or "").strip() - if not port: - return "/dev/serial/by-id" - if port.startswith("/"): - return port - if port.startswith("tty"): - return os.path.join("/dev", port) - return port - - @staticmethod - def _resolve_port_path(port_value: str) -> str: - """ - Resolve a usable serial device path. - - '/dev/serial/by-id' directory -> first entry inside it - - explicit device path -> returned unchanged - """ - if os.path.isdir(port_value): - try: - entries = sorted(os.listdir(port_value)) - except OSError: - return port_value - for entry in entries: - candidate = os.path.join(port_value, entry) - if os.path.exists(candidate): - return candidate - return port_value - - @staticmethod - def _checksum(data: bytes) -> int: - checksum = 0 - for b in data: - checksum ^= b - return checksum & 0xFF - - def __init__(self): - super().__init__("esp_serial_bridge") - - self.declare_parameter( - "esp_port", - "/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0", - ) - self.declare_parameter("baudrate", 115200) - self.declare_parameter("read_poll_hz", 100.0) - self.declare_parameter("reconnect_period_s", 2.0) - - configured_port = ( - self.get_parameter("esp_port").get_parameter_value().string_value - ) - self._port_config = self._normalize_port(configured_port) - self._port = self._resolve_port_path(self._port_config) - if configured_port != self._port_config: - self.get_logger().info( - f"Normalized serial port '{configured_port}' -> '{self._port_config}'" - ) - if self._port != self._port_config: - self.get_logger().info( - f"Resolved serial device from '{self._port_config}' -> '{self._port}'" - ) - self._baudrate = ( - self.get_parameter("baudrate").get_parameter_value().integer_value - ) - read_poll_hz = ( - self.get_parameter("read_poll_hz").get_parameter_value().double_value - ) - reconnect_period = ( - self.get_parameter("reconnect_period_s").get_parameter_value().double_value - ) - - self._serial = None - self._rx_buffer = bytearray() - self._reconnect_period = max(0.1, reconnect_period) - self._last_connect_attempt_ns = 0 - - self._sensor_pub = self.create_publisher( - EspSensorReadings, "/esp_sensor_readings", 10 - ) - self._polarimeter_pub = self.create_publisher( - PolarimeterSweep, "/esp_polarimeter_readings", 10 - ) - self.create_subscription( - PwmCommand, "/esp_pwm_command", self._on_pwm_command, qos_profile=3 - ) - - poll_period = 1.0 / max(1.0, read_poll_hz) - self.create_timer(poll_period, self._poll_serial) - - if serial is None: - self.get_logger().error( - "pyserial is not installed. Install python3-serial/pyserial." - ) - return - - self._ensure_serial_connected(force=True) - self.get_logger().info( - f"ESP serial bridge ready." f"port='{self._port}' baud={self._baudrate}" - ) - - def _ensure_serial_connected(self, force: bool = False) -> bool: - if serial is None: - return False - if self._serial is not None and self._serial.is_open: - return True - - now_ns = self.get_clock().now().nanoseconds - if not force and (now_ns - self._last_connect_attempt_ns) < int( - self._reconnect_period * 1e9 - ): - return False - self._last_connect_attempt_ns = now_ns - - try: - resolved_port = self._resolve_port_path(self._port_config) - if resolved_port != self._port: - self._port = resolved_port - self.get_logger().info(f"Using serial device '{self._port}'") - self._serial = serial.Serial( - port=self._port, baudrate=self._baudrate, timeout=0.0 - ) - self.get_logger().info(f"Connected to ESP serial port {self._port}") - return True - except SerialException as exc: - self._serial = None - self.get_logger().warn( - f"Failed to open serial port {self._port}: {exc}. Retrying..." - ) - return False - - def _close_serial(self): - if self._serial is not None: - try: - self._serial.close() - except Exception: - pass - self._serial = None - - def _on_pwm_command(self, msg: PwmCommand): - if not self._ensure_serial_connected(): - return - - payload = self._PWM_STRUCT.pack( - int(msg.pin) & 0xFF, - int(msg.type) & 0xFF, - int(msg.duty_cycle) & 0xFFFF, - int(msg.duration) & 0xFFFF, - int(msg.frequency) & 0xFFFF, - int(msg.ramp) & 0xFFFF, - ) - - checksum = self._checksum(payload) - - packet = ( - bytes( - [ - self._MAGIC0, - self._MAGIC1, - ] - ) - + payload - + bytes([checksum]) - ) - - try: - self._serial.write(packet) - - except (SerialException, OSError) as exc: - self.get_logger().warn(f"Serial write failed: {exc}") - self._close_serial() - - def _payload_len_for_type(self, frame_type: int) -> int | None: - if frame_type == self._TYPE_SENSORS: - return self._SENSOR_PAYLOAD_LEN - if frame_type == self._TYPE_POLAR: - return self._POLAR_PAYLOAD_LEN - return None - - def _poll_serial(self): - if not self._ensure_serial_connected(): - return - - try: - available = self._serial.in_waiting - if available > 0: - self._rx_buffer.extend(self._serial.read(available)) - except (SerialException, OSError) as exc: - self.get_logger().warn(f"Serial read failed: {exc}") - self._close_serial() - return - - # magic(2) + type(1) + payload + checksum(1) - while len(self._rx_buffer) >= 4: - if self._rx_buffer[0] != self._MAGIC0 or self._rx_buffer[1] != self._MAGIC1: - del self._rx_buffer[0] - continue - - frame_type = self._rx_buffer[2] - payload_len = self._payload_len_for_type(frame_type) - if payload_len is None: - del self._rx_buffer[0] - continue - - packet_size = 2 + 1 + payload_len + 1 - if len(self._rx_buffer) < packet_size: - break - - payload_start = 3 - payload_end = payload_start + payload_len - payload = bytes(self._rx_buffer[payload_start:payload_end]) - received_checksum = self._rx_buffer[payload_end] - expected_checksum = self._checksum(payload) - - if received_checksum != expected_checksum: - del self._rx_buffer[0] - continue - - del self._rx_buffer[:packet_size] - - if frame_type == self._TYPE_SENSORS: - methane, co2, temperature, moisture = self._SENSOR_STRUCT.unpack( - payload[: self._SENSOR_STRUCT.size] - ) - msg = EspSensorReadings() - msg.methane = methane - msg.co2 = co2 - msg.temperature = temperature - msg.moisture = moisture - self._sensor_pub.publish(msg) - elif frame_type == self._TYPE_POLAR: - readings = list( - self._POLAR_SWEEP_STRUCT.unpack( - payload[: self._POLAR_SWEEP_STRUCT.size] - ) - ) - msg = PolarimeterSweep() - msg.readings = readings - self._polarimeter_pub.publish(msg) - - def destroy_node(self): - self._close_serial() - super().destroy_node() - - -def main(args=None): - rclpy.init(args=args) - node = EspSerialBridge() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/HW-Devices/science_sensors/science_sensors/gas_sensor.py b/src/HW-Devices/science_sensors/science_sensors/gas_sensor.py deleted file mode 100755 index 904fa430..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/gas_sensor.py +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from adafruit_bme280 import basic as adafruit_bme280 -import adafruit_ens160 -import board - -from interfaces.msg import GasSensorReading - - -class GasSensor(Node): - """ - ROS 2 Node that reads environmental data (temperature, humidity, pressure, - CO2, and TVOC) from the BME280 and ENS160 sensors and publishes it to - the 'gas_sensor' topic. - """ - - def __init__(self): - """ - Initializes the GasSensor node, sets up I2C communication, configures - sensors (BME280 and ENS160), declares parameters, and sets up a timer - for publishing sensor readings. - """ - super().__init__("gas_sensor") - - i2c = board.I2C() - - self.declare_parameter("sea_level_pressure_hpa", 1013.25) - self.declare_parameter("gas_sensor_update_interval_s", 0.2) - - try: - self.bms280 = adafruit_bme280.Adafruit_BME280_I2C(i2c) - self.ens160 = adafruit_ens160.ENS160(i2c) - except RuntimeError as e: - raise RuntimeError("Gas Sensor Not Connected") from e - - self.bms280.sea_level_pressure = ( - self.get_parameter("sea_level_pressure_hpa") - .get_parameter_value() - .double_value - ) - self.bms280.mode = adafruit_bme280.MODE_NORMAL - - # self.ens160.reset() # not necessary AFAIK - self.ens160.mode = adafruit_ens160.MODE_STANDARD - - self.sensor_reading_pub = self.create_publisher( - GasSensorReading, "gas_sensor", 10 - ) - self.create_timer( - self.get_parameter("gas_sensor_update_interval_s") - .get_parameter_value() - .double_value, - self.loop, - ) - - def loop(self): - """ - Periodically reads data from the BME280 and ENS160 sensors, packages - the readings into a GasSensorReading message, and publishes it if - there are any subscribers. - """ - if self.sensor_reading_pub.get_subscription_count() > 0: - temperature = self.bms280.temperature - humidity = self.bms280.humidity - - reading = GasSensorReading() - reading.header.stamp = self.get_clock().now().to_msg() - reading.temperature_c = temperature - reading.pressure_pa = self.bms280.pressure * 100 # convert from hPa to Pa - reading.humidity_rh = humidity - - self.ens160.temperature_compensation = temperature - self.ens160.humidity_compensation = humidity - - reading.co2_ppm = self.ens160.eCO2 - reading.tvoc_ppb = self.ens160.TVOC - - self.sensor_reading_pub.publish(reading) - - -def main(args=None): - """ - Initializes the ROS 2 python library, starts this node, and enters the - ROS 2 spin loop to process incoming messages and trigger callbacks. - """ - rclpy.init(args=args) - node = GasSensor() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/HW-Devices/science_sensors/science_sensors/polarimeter.py b/src/HW-Devices/science_sensors/science_sensors/polarimeter.py deleted file mode 100644 index 33a00b82..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/polarimeter.py +++ /dev/null @@ -1,187 +0,0 @@ -#!/usr/bin/env python3 -import csv -import os -from datetime import datetime -from threading import Event -import matplotlib.pyplot as plt -import numpy as np - -from scipy.optimize import curve_fit - -import rclpy -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node - -from interfaces.msg import PolarimeterSweep, PwmCommand -from interfaces.srv import RunPolarimeter - -TYPE_POLAR = 0x02 - - -class PolarimeterNode(Node): - def __init__(self): - super().__init__("polarimeter") - self.declare_parameter("polar_pin", 27) - self.declare_parameter("polar_frequency", 50) - self.declare_parameter("output_dir", "/usr/local/zed/polar/") - self.declare_parameter("sweep_timeout_s", 90.0) - - self._pin = int( - self.get_parameter("polar_pin").get_parameter_value().integer_value - ) - self._frequency = int( - self.get_parameter("polar_frequency").get_parameter_value().integer_value - ) - self._output_dir = ( - self.get_parameter("output_dir").get_parameter_value().string_value - ) - self._timeout_s = ( - self.get_parameter("sweep_timeout_s").get_parameter_value().double_value - ) - - self._sweep_event = Event() - self._latest_sweep: PolarimeterSweep | None = None - self._service_cb_group = MutuallyExclusiveCallbackGroup() - - self._pwm_pub = self.create_publisher(PwmCommand, "/esp_pwm_command", 10) - self.create_subscription( - PolarimeterSweep, - "/esp_polarimeter_readings", - self._on_sweep, - qos_profile=10, - ) - self.create_service( - RunPolarimeter, - "run_polarimeter", - self._run_polarimeter, - callback_group=self._service_cb_group, - ) - - os.makedirs(self._output_dir, exist_ok=True) - self.get_logger().info( - f"Polarimeter service ready on 'run_polarimeter' " - f"(pin={self._pin}, freq={self._frequency} Hz, output_dir='{self._output_dir}')" - ) - - def _on_sweep(self, msg: PolarimeterSweep): - self._latest_sweep = msg - self._sweep_event.set() - - @staticmethod - def _write_csv(file_path: str, readings: list[int]) -> None: - with open(f"{file_path}.csv", "w", newline="") as csv_file: - writer = csv.writer(csv_file) - writer.writerow(["angle", "diff"]) - for angle, diff in enumerate(readings): - writer.writerow([angle, diff]) - - @staticmethod - def _write_graph(title: str, file_path: str, readings: list[int]) -> None: - x_deg = list(range(len(readings))) - x_rad = np.deg2rad(x_deg) - - # --- Model: A cos^2(x - phi) + C --- - def cos2_model(x, A, phi, C): - return A * np.cos(x - phi) ** 2 + C - - # --- Initial parameter guesses --- - A_guess = np.max(readings) - np.min(readings) - phi_guess = np.deg2rad(50) - C_guess = np.min(readings) - - popt, _ = curve_fit( - cos2_model, x_rad, readings, p0=[A_guess, phi_guess, C_guess] - ) - - A_fit, phi_fit, C_fit = popt - - # Convert phase to degrees - phase_deg = np.rad2deg(phi_fit) - - # --- Generate smooth fit curve --- - x_fit_deg = np.linspace(np.min(x_deg), np.max(x_deg), 1000) - x_fit_rad = np.deg2rad(x_fit_deg) - y_fit = cos2_model(x_fit_rad, A_fit, phi_fit, C_fit) - - # --- Plot --- - plt.figure() - plt.scatter(x_deg, readings, label="Data") - plt.plot(x_fit_deg, y_fit, label="Cos² Fit") - plt.axvline(phase_deg, linestyle="--", label=f"Phase Offset = {phase_deg:.2f}°") - - plt.xlabel("Angle (degrees)") - plt.ylabel("Signal") - plt.title(f"{title} - Cosine Squared Fit") - plt.legend() - plt.savefig(f"{file_path}.png") - - def _run_polarimeter(self, request, response): - title = (request.title or "").strip() - timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") - output_path = os.path.join(self._output_dir, f"polarimeter_{title}_{timestamp}") - - self._sweep_event.clear() - self._latest_sweep = None - - cmd = PwmCommand() - cmd.pin = self._pin & 0xFF - cmd.type = TYPE_POLAR - cmd.frequency = self._frequency & 0xFFFF - self._pwm_pub.publish(cmd) - self.get_logger().info( - f"Polarimeter sweep started (pin={cmd.pin}, waiting up to {self._timeout_s}s)" - ) - - if not self._sweep_event.wait(timeout=self._timeout_s): - response.success = False - response.message = ( - f"Timed out after {self._timeout_s}s waiting for polarimeter data" - ) - response.file_path = "" - return response - - sweep = self._latest_sweep - if sweep is None or not sweep.readings: - response.success = False - response.message = "Received empty polarimeter sweep" - response.file_path = "" - return response - - try: - output_dir = os.path.dirname(os.path.abspath(output_path)) - if output_dir: - os.makedirs(output_dir, exist_ok=True) - self._write_csv(output_path, list(sweep.readings)) - # self._write_graph(title, output_path, list(sweep.readings)) - except OSError as exc: - response.success = False - response.message = f"Failed to write CSV: {exc}" - response.file_path = "" - return response - - response.success = True - response.message = f"Saved {len(sweep.readings)} samples" - response.file_path = output_path - self.get_logger().info( - f"Polarimeter sweep saved to {output_path} ({len(sweep.readings)} samples)" - ) - return response - - -def main(args=None): - rclpy.init(args=args) - node = PolarimeterNode() - executor = MultiThreadedExecutor() - executor.add_node(node) - try: - executor.spin() - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/README.txt b/src/HW-Devices/science_sensors/science_sensors/science_esp_code/README.txt deleted file mode 100644 index 03f54d98..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/README.txt +++ /dev/null @@ -1,10 +0,0 @@ -Requires external DHT library (DHT sensor library by Adafruit in the ArduinoIDE library browser) -Compiled using ArduinoIDE. -Sensor pins are hard-coded to: - -METHANE_PIN = 4; -CO2_PIN = 12; -POLARIMETER_PIN = 14; -DHT_PIN = 2; - -Run the esp_serial_bridge ROS node to connect the esp to the ROS environment through matching topics. \ No newline at end of file diff --git a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino b/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino deleted file mode 100644 index 021a209c..00000000 --- a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino +++ /dev/null @@ -1,332 +0,0 @@ -#include -#include -#include -#include -#include -//#include - -#define TYPE_DC 0x00 -#define TYPE_SERVO 0x01 -#define TYPE_POLAR 0x02 - -#pragma pack(push, 1) -struct PwmCommand { - uint8_t pin; - uint8_t type; - uint16_t duty_cycle; - uint16_t duration; - uint16_t frequency; - uint16_t ramp; -}; - -struct SensorReadings { - uint16_t methane; - uint16_t co2; - float temperature; - float moisture; -}; -#pragma pack(pop) - -static const uint8_t MAGIC0 = 0xAA; -static const uint8_t MAGIC1 = 0x55; - -static const uint8_t PWM_RES_BITS = 10; -static const uint16_t PWM_MAX_DUTY = (1u << PWM_RES_BITS) - 1; -static const uint8_t PWM_MAX_ACTIVE = 8; -static const uint32_t PWM_TICK_MS = 10; - -static const uint32_t SENSOR_PERIOD_MS = 100; -static const uint32_t DHT_PERIOD_MS = 2000; - -static const int METHANE_PIN = 12; -static const int CO2_PIN = 33; -static const int DHT_PIN = 14; - -DHTesp dht; - -//Adafruit_NeoPixel led(1, 4, NEO_GRB + NEO_KHZ800); - -static QueueHandle_t g_pwmCmdQueue = nullptr; - -struct ActivePwm { - bool active; - uint8_t pin; - uint8_t type; - uint16_t duty_cycle; - uint16_t frequency; - uint32_t start_ms; - uint32_t ramp_end_ms; - uint32_t run_end_ms; - uint32_t start_duty; -}; - -static ActivePwm active_pins[PWM_MAX_ACTIVE]; - -static uint8_t calcChecksum(const uint8_t* data, size_t len) { - uint8_t c = 0; - for (size_t i = 0; i < len; i++) { - c ^= data[i]; - } - return c; -} - -static bool readFramedCommand(PwmCommand& cmd) { - static uint8_t state = 0; - static uint8_t payload[sizeof(PwmCommand)]; - static size_t payload_index = 0; - - while (Serial.available() > 0) { - uint8_t b = Serial.read(); - - switch (state) { - case 0: - if (b == MAGIC0) { - state = 1; - } - break; - - case 1: - if (b == MAGIC1) { - payload_index = 0; - state = 2; - } else if (b == MAGIC0) { - state = 1; - } else { - state = 0; - } - break; - - case 2: - payload[payload_index++] = b; - if (payload_index >= sizeof(PwmCommand)) { - state = 3; - } - break; - - case 3: { - uint8_t expected = calcChecksum(payload, sizeof(PwmCommand)); - uint8_t received = b; - - state = 0; - payload_index = 0; - - if (received == expected) { - memcpy(&cmd, payload, sizeof(PwmCommand)); - return true; - } - - break; - } - - default: - state = 0; - payload_index = 0; - break; - } - } - - return false; -} - -#define TYPE_SENSORS 0x01 -#define TYPE_POLAR 0x02 - -static void writeFramedSensorReadings(const uint8_t* payload, size_t len, uint8_t type) { - uint8_t checksum = calcChecksum(payload, len); - - Serial.write(MAGIC0); - Serial.write(MAGIC1); - Serial.write(type); - Serial.write(payload, len); - Serial.write(checksum); -} - -static int findPWMIndex(uint8_t pin) { - for (int i = 0; i < PWM_MAX_ACTIVE; ++i) { - if (active_pins[i].active && active_pins[i].pin == pin) return i; - } - return -1; -} - -static int findFreeIndex() { - for (int i = 0; i < PWM_MAX_ACTIVE; ++i) { - if (!active_pins[i].active) return i; - } - return -1; -} - -static void releaseIndex(int i) { - if (i < 0 || i >= PWM_MAX_ACTIVE) return; - if (!active_pins[i].active) return; - ledcWrite(active_pins[i].pin, 0); - ledcDetach(active_pins[i].pin); - active_pins[i].active = false; -} - -static void runCommand(const PwmCommand& cmd) { - uint16_t duty = cmd.duty_cycle; - if (duty > PWM_MAX_DUTY) duty = PWM_MAX_DUTY; - - int i = findPWMIndex(cmd.pin); - if (i < 0) i = findFreeIndex(); - if (i < 0) return; - - if (!active_pins[i].active) { - if (!ledcAttach(cmd.pin, cmd.frequency, PWM_RES_BITS)) { - return; - } - } else if (cmd.frequency != active_pins[i].frequency) { - releaseIndex(i); - - if (!ledcAttach(cmd.pin, cmd.frequency, PWM_RES_BITS)) { - return; - } - } - - if (cmd.type == TYPE_SERVO) { - active_pins[i].active = true; - active_pins[i].type = TYPE_SERVO; - active_pins[i].pin = cmd.pin; - active_pins[i].duty_cycle = duty; - active_pins[i].frequency = cmd.frequency; - ledcWrite(cmd.pin, duty); - } else if (cmd.type == TYPE_DC) { - uint32_t now = millis(); - uint32_t rampMs = (uint32_t)cmd.ramp * 100u; - uint32_t holdMs = (uint32_t)cmd.duration * 100u; - - active_pins[i].active = true; - active_pins[i].type = TYPE_DC; - active_pins[i].start_duty = active_pins[i].duty_cycle; - active_pins[i].pin = cmd.pin; - active_pins[i].duty_cycle = duty; - active_pins[i].start_ms = now; - active_pins[i].ramp_end_ms = now + rampMs; - active_pins[i].run_end_ms = active_pins[i].ramp_end_ms + holdMs; - active_pins[i].frequency = cmd.frequency; - - if (rampMs == 0) { - ledcWrite(cmd.pin, duty); - } - } else if (cmd.type == TYPE_POLAR) { - active_pins[i].active = true; - active_pins[i].type = TYPE_POLAR; - active_pins[i].pin = cmd.pin; - active_pins[i].duty_cycle = duty; - active_pins[i].frequency = cmd.frequency; - runPolarimeter(cmd.pin); - releaseIndex(i); - } -} - -static void updateActivePwm() { - uint32_t now = millis(); - for (int i = 0; i < PWM_MAX_ACTIVE; ++i) { - if (!active_pins[i].active) continue; - if (active_pins[i].type == TYPE_SERVO) continue; - if (active_pins[i].type == TYPE_POLAR) continue; - ActivePwm& a = active_pins[i]; - - if ((int32_t)(now - a.run_end_ms) >= 0) { - releaseIndex(i); - continue; - } - - if ((int32_t)(now - a.ramp_end_ms) >= 0) { - ledcWrite(a.pin, a.duty_cycle); - continue; - } - - uint32_t elapsed = now - a.start_ms; - uint32_t rampSpan = a.ramp_end_ms - a.start_ms; - uint32_t rampDelta = (uint32_t)a.duty_cycle - a.start_duty; - uint32_t scaled = rampDelta * elapsed / rampSpan + a.start_duty; - if (scaled > a.duty_cycle) scaled = a.duty_cycle; - ledcWrite(a.pin, scaled); - } -} - -static void pwmTask(void* /*arg*/) { - TickType_t lastWake = xTaskGetTickCount(); - for (;;) { - PwmCommand cmd; - while (xQueueReceive(g_pwmCmdQueue, &cmd, 0) == pdTRUE) { - runCommand(cmd); - } - updateActivePwm(); - vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(PWM_TICK_MS)); - } -} - -static uint16_t readMethane() { return (uint16_t)analogRead(METHANE_PIN); } -static uint16_t readCo2() { return (uint16_t)analogRead(CO2_PIN); } - -static void sensorTask(void* /*arg*/) { - static const int DHT_INTERVAL = DHT_PERIOD_MS / SENSOR_PERIOD_MS; - static int dht_counter = 0; - static float lastTemperature = 0; - static float lastMoisture = 0; - TickType_t lastWake = xTaskGetTickCount(); - for (;;) { - SensorReadings pkt; - pkt.methane = readMethane(); - pkt.co2 = readCo2(); - if (++dht_counter >= DHT_INTERVAL) { - dht_counter = 0; - TempAndHumidity data = dht.getTempAndHumidity(); - lastTemperature = data.temperature; - lastMoisture = data.humidity; - } - pkt.temperature = lastTemperature; - pkt.moisture = lastMoisture; - writeFramedSensorReadings(reinterpret_cast(&pkt), sizeof(SensorReadings), TYPE_SENSORS); - - vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(SENSOR_PERIOD_MS)); - } -} - -const int POLAR_MIN_MS = 700; -const int POLAR_MAX_MS = 2200; -const int POLAR_MAX_ANGLE = 180; -const int POLAR_SENSOR_PIN = 25; - -static void runPolarimeter(uint8_t pin) { - int sensor_readings[POLAR_MAX_ANGLE + 1] = {0}; - int pwmAngle = 0; - ledcWrite(pin, map(POLAR_MIN_MS, 0, 20000, 0, PWM_MAX_DUTY)); - delay(3000); - while (pwmAngle <= POLAR_MAX_ANGLE) { - int ms = map(pwmAngle, 0, POLAR_MAX_ANGLE, POLAR_MIN_MS, POLAR_MAX_MS); - ledcWrite(pin, map(ms, 0, 20000, 0, PWM_MAX_DUTY)); - sensor_readings[pwmAngle] = analogRead(POLAR_SENSOR_PIN); - delay(200); - pwmAngle++; - } - writeFramedSensorReadings(reinterpret_cast(sensor_readings), (POLAR_MAX_ANGLE + 1) * sizeof(int), TYPE_POLAR); -} - -void setup() { - Serial.begin(115200); - dht.setup(DHT_PIN, DHTesp::DHT22); - //led.begin(); - //led.setPixelColor(0, led.Color(255, 0, 255)); - //led.show(); - - for (int i = 0; i < PWM_MAX_ACTIVE; ++i) { - active_pins[i].active = false; - } - - g_pwmCmdQueue = xQueueCreate(8, sizeof(PwmCommand)); - - xTaskCreatePinnedToCore(pwmTask, "pwmTask", 4096, nullptr, 2, nullptr, 1); - xTaskCreatePinnedToCore(sensorTask, "sensorTask", 4096, nullptr, 1, nullptr, 0); -} - -void loop() { - PwmCommand cmd; - - while (readFramedCommand(cmd)) { - xQueueSend(g_pwmCmdQueue, &cmd, pdMS_TO_TICKS(20)); - } - vTaskDelay(pdMS_TO_TICKS(1)); -} diff --git a/src/HW-Devices/science_sensors/setup.py b/src/HW-Devices/science_sensors/setup.py index b58025fb..1bbaa5e6 100644 --- a/src/HW-Devices/science_sensors/setup.py +++ b/src/HW-Devices/science_sensors/setup.py @@ -30,10 +30,7 @@ license="TODO: License declaration", entry_points={ "console_scripts": [ - "gas_sensor = science_sensors.gas_sensor:main", "panoramic = science_sensors.panoramic:main", - "esp_serial_bridge = science_sensors.esp_serial_bridge:main", - "polarimeter = science_sensors.polarimeter:main", ], }, ) diff --git a/src/HW-Devices/science_sensors/src/main.cpp b/src/HW-Devices/science_sensors/src/main.cpp new file mode 100644 index 00000000..b05a1be4 --- /dev/null +++ b/src/HW-Devices/science_sensors/src/main.cpp @@ -0,0 +1,19 @@ +#include "epoll_event_loop.hpp" +#include "science_node.hpp" +#include "socket_can.hpp" +#include + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + EpollEventLoop event_loop; + auto can_node = std::make_shared("science_node"); + + if (!can_node->init(&event_loop)) + return -1; + + std::thread can_event_loop([&event_loop]() { event_loop.run_until_empty(); }); + rclcpp::spin(can_node); + can_node->deinit(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/HW-Devices/science_sensors/src/science_node.cpp b/src/HW-Devices/science_sensors/src/science_node.cpp new file mode 100644 index 00000000..daee497f --- /dev/null +++ b/src/HW-Devices/science_sensors/src/science_node.cpp @@ -0,0 +1,277 @@ +#include "science_node.hpp" + +#include +#include + +enum CmdId : uint32_t { + SetMotor = 0x00, + SetServo = 0x01, + PolarScan = 0x02, + ADCData = 0x03, + TempData = 0x04, + CO2Data = 0x05, + PolarData = 0x06 +}; + +ScienceNode::ScienceNode(const std::string name, + const rclcpp::NodeOptions &options) + : rclcpp::Node(name, options) { + this->declare_parameter("interface", "can0"); + this->declare_parameter("node_id", 30); + this->declare_parameter("sweep_timeout", 60); + this->declare_parameter("output_dir", "/usr/local/zed/polar/"); + + mkdir(this->get_parameter("output_dir").as_string().c_str(), 0777); + + this->adc_pub_ = + this->create_publisher("/science/adc", 10); + this->temp_pub_ = + this->create_publisher("/science/temp", 10); + this->co2_pub_ = + this->create_publisher("/science/co2", 10); + this->polar_pub_ = this->create_publisher( + "/science/polarimeter", 10); + + this->motor_sub_ = this->create_subscription( + "/science/motor", rclcpp::QoS(1).reliable(), + [this](const interfaces::msg::ScienceMotor::SharedPtr msg) { + std::lock_guard guard(motor_mutex_); + motor_msg_ = *msg; + motor_evt_.set(); + }); + + this->servo_sub_ = this->create_subscription( + "/science/servo", rclcpp::QoS(1).reliable(), + [this](const interfaces::msg::ScienceServo::SharedPtr msg) { + std::lock_guard guard(servo_mutex_); + servo_msg_ = *msg; + servo_evt_.set(); + }); + + this->polar_srv_ = this->create_service( + "/science/run_polarimeter", + std::bind(&ScienceNode::polar_callback, this, std::placeholders::_1, + std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "Science Sensors Node started"); +} + +void ScienceNode::deinit() { + motor_evt_.deinit(); + servo_evt_.deinit(); + req_polar_evt_.deinit(); + can_intf_.deinit(); +} + +bool ScienceNode::init(EpollEventLoop *event_loop) { + node_id_ = this->get_parameter("node_id").as_int(); + std::string interface = this->get_parameter("interface").as_string(); + + if (!can_intf_.init(interface, event_loop, + std::bind(&ScienceNode::recv_callback, this, + std::placeholders::_1))) { + RCLCPP_ERROR(this->get_logger(), + "Failed to initialize socket can interface: %s", + interface.c_str()); + return false; + } + if (!motor_evt_.init(event_loop, + std::bind(&ScienceNode::motor_callback, this))) { + RCLCPP_ERROR(this->get_logger(), + "Failed to initialize motor subscriber event"); + return false; + } + if (!servo_evt_.init(event_loop, + std::bind(&ScienceNode::servo_callback, this))) { + RCLCPP_ERROR(this->get_logger(), + "Failed to initialize servo subscriber event"); + return false; + } + if (!req_polar_evt_.init(event_loop, + std::bind(&ScienceNode::request_polar, this))) { + RCLCPP_ERROR(this->get_logger(), + "Failed to initialize polarimeter request event"); + return false; + } + RCLCPP_INFO(this->get_logger(), "node_id: %d", node_id_); + RCLCPP_INFO(this->get_logger(), "interface: %s", interface.c_str()); + return true; +} + +void ScienceNode::recv_callback(const can_frame &frame) { + if (((frame.can_id >> 5) & 0x3F) != node_id_) + return; + + switch (frame.can_id & 0x1F) { + case CmdId::ADCData: { + if (!verify_length("ADCData", 6, frame.can_dlc)) + break; + interfaces::msg::ScienceADC msg; + msg.adc1 = (frame.data[0] << 8) | frame.data[1]; + msg.adc2 = (frame.data[2] << 8) | frame.data[3]; + msg.adc3 = (frame.data[4] << 8) | frame.data[5]; + if (adc_pub_) { + adc_pub_->publish(msg); + } + break; + } + case CmdId::TempData: { + if (!verify_length("TempData", 8, frame.can_dlc)) + break; + interfaces::msg::DHT22 msg; + uint32_t temp_bytes = (frame.data[0] << 24) | (frame.data[1] << 16) | + (frame.data[2] << 8) | frame.data[3]; + uint32_t humid_bytes = (frame.data[4] << 24) | (frame.data[5] << 16) | + (frame.data[6] << 8) | frame.data[7]; + memcpy(&msg.temperature, &temp_bytes, sizeof(temp_bytes)); + memcpy(&msg.humidity, &humid_bytes, sizeof(humid_bytes)); + if (temp_pub_) { + temp_pub_->publish(msg); + } + break; + } + case CmdId::CO2Data: { + if (!verify_length("CO2Data", 2, frame.can_dlc)) + break; + std_msgs::msg::UInt16 msg; + msg.data = (frame.data[0] << 8) | frame.data[1]; + if (co2_pub_) { + co2_pub_->publish(msg); + } + break; + } + case CmdId::PolarData: { + if (!verify_length("PolarData", 7, frame.can_dlc)) + break; + std::lock_guard guard(polar_mutex_); + if (frame.data[0] != polar_index_) + RCLCPP_WARN(this->get_logger(), + "Polarimeter indexing mismatch: expected packet starting at " + "index %d, got %d", + polar_index_, frame.data[0]); + polar_index_ = frame.data[0]; + polar_points_[polar_index_++] = (frame.data[1] << 8) | frame.data[2]; + polar_points_[polar_index_++] = (frame.data[3] << 8) | frame.data[4]; + polar_points_[polar_index_++] = (frame.data[5] << 8) | frame.data[6]; + + if (polar_index_ == SCAN_STEPS) { + polar_cond_.notify_one(); + } + break; + } + case CmdId::SetMotor: + case CmdId::SetServo: + case CmdId::PolarScan: { + break; // Ignore commands coming from another master/host on the bus + } + default: { + RCLCPP_WARN(this->get_logger(), "Received unused message: ID = 0x%x", + (frame.can_id & 0x1F)); + break; + } + } +} + +void ScienceNode::motor_callback() { + struct can_frame frame; + frame.can_id = node_id_ << 5 | CmdId::SetMotor; + { + std::lock_guard guard(motor_mutex_); + frame.data[0] = motor_msg_.pin; + frame.data[1] = motor_msg_.duty_cycle; + frame.data[2] = (motor_msg_.duration >> 8) & 0xff; + frame.data[3] = motor_msg_.duration & 0xff; + frame.data[4] = (motor_msg_.ramp >> 8) & 0xff; + frame.data[5] = motor_msg_.ramp & 0xff; + } + frame.can_dlc = 6; + can_intf_.send_can_frame(frame); +} + +void ScienceNode::servo_callback() { + struct can_frame frame; + frame.can_id = node_id_ << 5 | CmdId::SetServo; + { + std::lock_guard guard(servo_mutex_); + frame.data[0] = servo_msg_.pin; + frame.data[1] = (servo_msg_.us >> 8) & 0xff; + frame.data[2] = servo_msg_.us & 0xff; + } + frame.can_dlc = 3; + can_intf_.send_can_frame(frame); +} + +void ScienceNode::polar_callback( + const std::shared_ptr request, + std::shared_ptr response) { + { + std::unique_lock guard(polar_mutex_); + for (int i = 0; i < SCAN_STEPS; i++) { + polar_points_[i] = 0; + } + polar_index_ = 0; + } + req_polar_evt_.set(); + + std::unique_lock guard(polar_mutex_); + if (polar_cond_.wait_for( + guard, std::chrono::seconds( + this->get_parameter("sweep_timeout").as_int())) == + std::cv_status::timeout) { + response->success = false; + response->message = "Timeout waiting for polarimeter samples"; + return; + } else { + if (polar_index_ != SCAN_STEPS) { + RCLCPP_WARN(this->get_logger(), + "Polar tried writing without a full buffer"); + response->success = false; + response->message = "Tried writing without a full sample buffer"; + return; + } + if (this->polar_pub_) { + interfaces::msg::PolarimeterSweep msg; + msg.readings.assign(polar_points_, polar_points_ + SCAN_STEPS); + this->polar_pub_->publish(msg); + } + + char path[128]; + snprintf(path, sizeof(path), "%spolarimeter_%s_%.0f.csv", + this->get_parameter("output_dir").as_string().c_str(), + request->title.c_str(), this->now().seconds()); + int fd = open(path, O_CREAT | O_WRONLY, 0777); + + char contents[1024]; + uint16_t pos = 0; + + pos += snprintf(contents, sizeof(contents) - pos, "step, reading\n"); + + for (int i = 0; i < SCAN_STEPS; i++) { + pos += snprintf(contents + pos, sizeof(contents) - pos, "%d, %d\n", i, + polar_points_[i]); + } + write(fd, contents, pos - 1); + close(fd); + response->success = true; + response->message = "Polarimeter samples saved"; + response->file_path = path; + RCLCPP_INFO(this->get_logger(), "Polarimeter sweep saved to %s", path); + } +} + +void ScienceNode::request_polar() { + struct can_frame frame; + frame.can_id = node_id_ << 5 | CmdId::PolarScan; + frame.can_dlc = 0; + can_intf_.send_can_frame(frame); +} + +inline bool ScienceNode::verify_length(const std::string &name, + uint8_t expected, uint8_t length) { + bool valid = expected == length; + RCLCPP_DEBUG(this->get_logger(), "received %s", name.c_str()); + if (!valid) + RCLCPP_WARN(this->get_logger(), "Incorrect %s frame length: %d != %d", + name.c_str(), length, expected); + return valid; +} \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/config/3dpro.yaml b/src/Teleop-Control/joystick_control/config/3dpro.yaml index e3e75fa8..7dfa7f6f 100644 --- a/src/Teleop-Control/joystick_control/config/3dpro.yaml +++ b/src/Teleop-Control/joystick_control/config/3dpro.yaml @@ -32,7 +32,7 @@ arm_teleop_node: drill_teleop_node: ros__parameters: - drill_power_axis: 2 + drill_power_axis: 3 drill_elevation_axis: 1 joy_first_message_timeout_s: 10.0 max_drill_duty: 1.0 diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index f8d5d9e6..9668a0ce 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -20,15 +20,17 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/ArucoBoard.msg" "msg/ArucoMarkers.msg" + "msg/DHT22.msg" "msg/Distance.msg" - "msg/EspSensorReadings.msg" "msg/GasSensorReading.msg" "msg/MoveGroupStatus.msg" "msg/NodeList.msg" "msg/ObjectDetected.msg" "msg/PolarimeterSweep.msg" - "msg/PwmCommand.msg" "msg/RtpStats.msg" + "msg/ScienceADC.msg" + "msg/ScienceMotor.msg" + "msg/ScienceServo.msg" "msg/SrtStats.msg" "msg/SvinStatus.msg" "msg/SystemTelemetry.msg" diff --git a/src/interfaces/msg/DHT22.msg b/src/interfaces/msg/DHT22.msg new file mode 100644 index 00000000..58beda1d --- /dev/null +++ b/src/interfaces/msg/DHT22.msg @@ -0,0 +1,2 @@ +float32 temperature +float32 humidity \ No newline at end of file diff --git a/src/interfaces/msg/EspSensorReadings.msg b/src/interfaces/msg/EspSensorReadings.msg deleted file mode 100644 index 57b06469..00000000 --- a/src/interfaces/msg/EspSensorReadings.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint16 methane -uint16 co2 -uint16 polarimeter -float32 temperature -float32 moisture diff --git a/src/interfaces/msg/PolarimeterSweep.msg b/src/interfaces/msg/PolarimeterSweep.msg index 15b1997e..907446bd 100644 --- a/src/interfaces/msg/PolarimeterSweep.msg +++ b/src/interfaces/msg/PolarimeterSweep.msg @@ -1,2 +1,2 @@ -# One full polarimeter sweep (181 samples, angles 0-180). -int32[] readings +# One full polarimeter sweep +uint32[] readings diff --git a/src/interfaces/msg/ScienceADC.msg b/src/interfaces/msg/ScienceADC.msg new file mode 100644 index 00000000..df36ec43 --- /dev/null +++ b/src/interfaces/msg/ScienceADC.msg @@ -0,0 +1,3 @@ +uint16 adc1 +uint16 adc2 +uint16 adc3 \ No newline at end of file diff --git a/src/interfaces/msg/PwmCommand.msg b/src/interfaces/msg/ScienceMotor.msg similarity index 66% rename from src/interfaces/msg/PwmCommand.msg rename to src/interfaces/msg/ScienceMotor.msg index 5441dc63..1eaf407e 100644 --- a/src/interfaces/msg/PwmCommand.msg +++ b/src/interfaces/msg/ScienceMotor.msg @@ -1,6 +1,4 @@ uint8 pin -uint8 type uint16 duty_cycle uint16 duration -uint16 frequency uint16 ramp diff --git a/src/interfaces/msg/ScienceServo.msg b/src/interfaces/msg/ScienceServo.msg new file mode 100644 index 00000000..669cb94e --- /dev/null +++ b/src/interfaces/msg/ScienceServo.msg @@ -0,0 +1,2 @@ +uint8 pin +uint16 us \ No newline at end of file