mm_utils.cpp 3.66 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
#include "mm_controller/mm_utils.hpp"

namespace mm
{
    //Get device name from model based on device types from API.
    std::string GetDeviceModelFromDeviceID(const uint8_t fwVerDeviceType)
    {
        switch (fwVerDeviceType)
        {
        case 22:
            return "Beacon HW V4.5";
        case 23:
            return "Beacon HW V4.5 (hedgehog mode)";
        case 24:
            return "Modem HW V4.9";
        case 30:
            return "Beacon HW V4.9";
        case 31:
            return "Beacon HW V4.9 (hedgehog mode)";
        case 32:
            return "Beacon Mini-RX";
        case 36:
            return "Beacon Mini-TX";
        case 37:
            return "Beacon-TX-IP67";
        case 41:
            return "Beacon industrial-RX";
        case 42:
            return "Super-Beacon";
        case 43:
            return "Super-Beacon (hedgehog mode)";
        case 44:
            return "Super-Beacon industrial";
        case 45:
            return "Super-Beacon industrial (hedgehog mode)";
        case 46:
            return "Super-Modem";
        case 48:
            return "Modem HW V5.1";
        default:
            return "Unknown device.";
        }
    }
    std::string ComputeFirmwareString(const uint8_t major, const uint8_t minor, const uint8_t minor2)
    {
        std::string v_string = "v" + std::to_string(major) + "." + std::to_string(minor);
        if (!minor2 == 0)
            v_string = v_string + std::to_string(minor2);
        return v_string;
    }

    void PrintDeviceInformation(const rclcpp::Logger logger, const MarvelmindDeviceVersion dev)
    {
        RCLCPP_INFO(logger, "Firmware: " + ComputeFirmwareString(dev.fwVerMajor, dev.fwVerMinor, dev.fwVerMinor2));
        RCLCPP_INFO(logger, "CPU ID: " + mm::int_to_hex(dev.cpuId));
    }

levi's avatar
levi committed
58
59
60
61
    void PrintDeviceDistances(const rclcpp::Logger logger, const std::vector<mm_controller_interfaces::msg::DeviceDistance> distances)
    {
        for (size_t i = 0; i < distances.size(); i++)
        {
levi's avatar
levi committed
62
            RCLCPP_INFO(logger, "AddrRx: %d AddrTx: %d Distance: %d", distances[i].dev_adr_rx, distances[i].dev_adr_tx, distances[i].distance_mm);
levi's avatar
levi committed
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
        }
    }
    void PrintDeviceLocation(const rclcpp::Logger logger, const std::vector<mm_controller_interfaces::msg::DeviceStatus> devices)
    {
        for (size_t i = 0; i < devices.size(); i++)
        {
            if (devices[i].dev_is_sleeping) break;
            RCLCPP_INFO(logger, "devAddr:%d    X: %d Y: %d", devices[i].dev_deviceaddress, devices[i].loc_x_pos_mm, devices[i].loc_y_pos_mm);
        }
    }

    std::vector<mm_controller_interfaces::msg::DeviceDistance> FilterDistances(const std::vector<mm_controller_interfaces::msg::DeviceDistance> distances)
    {
        std::vector<mm_controller_interfaces::msg::DeviceDistance> retDistances;
        for (size_t i = 0; i < distances.size(); i++)
        {
            if (distances[i].dev_adr_rx != 0 || distances[i].dev_adr_tx != 0)
            {
                retDistances.push_back(distances[i]);
            }
        }
        return retDistances;
    }

87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
    template <typename T>
    std::string int_to_hex(T i)
    {
        std::stringstream stream;
        // stream << "0x"
        //        << std::setfill('0') << std::setw(sizeof(T) * 2)
        //        << std::hex << i;
        stream << std::hex << i;
        return stream.str();
    }
    std::string GenerateExecutionResult(bool executionSucces, std::string msg)
    {
        std::toupper(msg[0]);
        msg.append(".");
        std::string retMsg = "Execution";
        if (executionSucces)
            return retMsg.append(" succeeded.").append(" ").append(msg);
        return retMsg.append(" failed.").append(" ").append(msg);
    }
}