ros1 core2 mbed6 test - rpi4 connection tests

ros1_core2_mbed6_test.cpp

#include <mbed.h>
#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_srvs/SetBool.h>

#define TEMP_SENSOR_AVG_SLOPE_MV_PER_CELSIUS 2.5f
#define TEMP_SENSOR_VOLTAGE_MV_AT_25 760.0f
#define ADC_REFERENCE_VOLTAGE 3.3f
#define ADC_MAX_VALUE 4095.0f
#define TEMP110 110.0f
#define TEMP30 30.0f
#define ADC_TEMP_3V3_30C_RAW_ADDR 0x1FFF7A2CU
#define ADC_TEMP_3V3_110C_RAW_ADDR 0x1FFF7A2EU

constexpr std::chrono::milliseconds SENSOR_INTER_MEASUREMENT_MS{1000ms};
constexpr std::chrono::milliseconds SPIN_DELAY_MS{10ms};

class RosInternalSensor : private ros::NodeHandle, private NonCopyable<RosInternalSensor>
{
public:
    /**
     * @brief Initialise ros mock sensor node.
     */
    void init()
    {
        // initialise ros node
        initNode();

        const uint16_t * const TS_CAL1_RAW = reinterpret_cast<uint16_t*>(ADC_TEMP_3V3_30C_RAW_ADDR);
        const uint16_t * const TS_CAL2_RAW = reinterpret_cast<uint16_t*>(ADC_TEMP_3V3_110C_RAW_ADDR);

        _ts_cal1_3v3_30 = static_cast<float>(*TS_CAL1_RAW) * (ADC_REFERENCE_VOLTAGE / ADC_MAX_VALUE);
        _ts_cal2_3v3_110 = static_cast<float>(*TS_CAL2_RAW) * (ADC_REFERENCE_VOLTAGE / ADC_MAX_VALUE);

        // prepare sensor data
        _sensor_msg.data = 0.0f;
        _sensor_reader.set_reference_voltage(ADC_REFERENCE_VOLTAGE);

        // advertise publisher
        advertise(_sensor_pub1);
        advertise(_sensor_pub2);

        // advertise service
        advertiseService(_l1_srv);
        advertiseService(_l2_srv);
        advertiseService(_l3_srv);

        char buffer[64];

        snprintf(buffer, 64, "TS_CAL1_RAW = 0x%0X, %u", *TS_CAL1_RAW, *TS_CAL1_RAW);

        loginfo(buffer);

        snprintf(buffer, 64, "TS_CAL2_RAW = 0x%0X, %u", *TS_CAL2_RAW, *TS_CAL2_RAW);

        loginfo(buffer);

    };

    /**
     * @brief Run ros mock sensor node test.
     */
    void spin()
    {
        auto wake_time = Kernel::Clock::now();
        auto publish_time = wake_time + SENSOR_INTER_MEASUREMENT_MS; 

        while (1)
        {
            auto time = Kernel::Clock::now();

            if (time >= publish_time)
            {
                // write log message
                loginfo("CORE2: reading mcu temp...");

                // internal temperature sensor value using the formula
                float adc_reading = _sensor_reader.read_voltage();
                _sensor_msg.data = ((adc_reading * 1000.0 - TEMP_SENSOR_VOLTAGE_MV_AT_25)/TEMP_SENSOR_AVG_SLOPE_MV_PER_CELSIUS) + 25.0;
                _sensor_pub1.publish(&_sensor_msg);

                // internal temperature sensor value using calibration values and linear approximation
                _sensor_msg.data = 80.0f * (adc_reading - _ts_cal1_3v3_30)/(_ts_cal2_3v3_110 - _ts_cal1_3v3_30) + 30.0f;
                _sensor_pub2.publish(&_sensor_msg);

                publish_time = wake_time + SENSOR_INTER_MEASUREMENT_MS; 
            }

            spinOnce();

            wake_time+=SPIN_DELAY_MS;

            ThisThread::sleep_until(wake_time);
        }
    };

    void enableL1(const std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
    {
        if(req.data)
        {
            _l1 = 1;
            res.message = "L1 ON";
        }
        else
        {
            _l1 = 0;
            res.message = "L1 OFF";
        }
        res.success = true;
    }

    void enableL2(const std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
    {
        if(req.data)
        {
            _l2 = 1;
            res.message = "L2 ON";
        }
        else
        {
            _l2 = 0;
            res.message = "L2 OFF";
        }
        res.success = true;
    }
    
    void enableL3(const std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
    {
        if(req.data)
        {
            _l3 = 1;
            res.message = "L3 ON";
        }
        else
        {
            _l3 = 0;
            res.message = "L3 OFF";
        }
        res.success = true;
    }

private:
    float _ts_cal1_3v3_30{0.0f};
    float _ts_cal2_3v3_110{0.0f};
    DigitalOut _l1{LED1, 0};
    DigitalOut _l2{LED2, 0};
    DigitalOut _l3{LED3, 0};
    AnalogIn _sensor_reader{ADC_TEMP};
    std_msgs::Float32 _sensor_msg{};
    ros::Publisher _sensor_pub1{"~mcu_temp_type1", &_sensor_msg};
    ros::Publisher _sensor_pub2{"~mcu_temp_type2", &_sensor_msg};
    ros::ServiceServer<std_srvs::SetBool::Request, std_srvs::SetBool::Response, RosInternalSensor> _l1_srv{"l1",&RosInternalSensor::enableL1, this};
    ros::ServiceServer<std_srvs::SetBool::Request, std_srvs::SetBool::Response, RosInternalSensor> _l2_srv{"l2",&RosInternalSensor::enableL2, this};
    ros::ServiceServer<std_srvs::SetBool::Request, std_srvs::SetBool::Response, RosInternalSensor> _l3_srv{"l3",&RosInternalSensor::enableL3, this};
};

int main()
{
    RosInternalSensor thermostat_sensor;
    thermostat_sensor.init();
    thermostat_sensor.spin();

    return 0;
}