mm_node.cpp 22.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#include "mm_controller/mm_node.hpp"
#include "mm_controller/mm_utils.hpp"

namespace mm
{
    MMNode::MMNode(const std::string &node_name, bool intra_process_comms)
        : rclcpp_lifecycle::LifecycleNode(node_name, rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms))
    {
        //Declare parameters
        _paramDevice = this->declare_parameter("device", "/dev/ttyACM0");
        _paramStatusPublisherTopicName = this->declare_parameter("statusPublisherTopicName", "status");
        _paramRobotBeaconPublisherTopicName = this->declare_parameter("robotBeaconPublisherTopicName", "position");
        _paramServicePrefixName = this->declare_parameter("servicePrefixName", "marvelmind_controller");
        _paramWakeDeviceServiceName = this->declare_parameter("wakeDeviceServiceName", "device_wake");
        _paramSleepDeviceServiceName = this->declare_parameter("sleepDeviceServiceName", "device_sleep");
        _paramRestartDeviceServiceName = this->declare_parameter("restartDeviceServiceName", "device_restart");
17
        _paramDefaultSubmapId = this->declare_parameter("submapId", 0);
18
19
20
21
22
23

        _statusMsg = mm_controller_interfaces::msg::MMControllerStatus();
        std::string
            wakeName = this->_paramServicePrefixName,
            sleepName = this->_paramServicePrefixName,
            restartName = this->_paramServicePrefixName;
24
        _wakeDeviceSrvPtr = this->create_service<mm_controller_interfaces::srv::NodeExecution>(
25
26
27
28
29
30
            wakeName.append("/").append(_paramWakeDeviceServiceName),
            std::bind(
                &mm::MMNode::WakeDeviceCallback,
                this,
                std::placeholders::_1,   //request placeholder
                std::placeholders::_2)); //response placeholder
31
        _sleepDeviceSrvPtr = this->create_service<mm_controller_interfaces::srv::NodeExecution>(
32
33
34
35
36
37
            sleepName.append("/").append(_paramSleepDeviceServiceName),
            std::bind(
                &mm::MMNode::SleepDeviceCallback,
                this,
                std::placeholders::_1, //request placeholder
                std::placeholders::_2));
38
        _restartDeviceSrvPtr = this->create_service<mm_controller_interfaces::srv::NodeExecution>(
39
40
41
42
43
44
            restartName.append("/").append(_paramRestartDeviceServiceName),
            std::bind(
                &mm::MMNode::RestartDeviceCallback,
                this,
                std::placeholders::_1,
                std::placeholders::_2));
45
46
47
48
49

        _freezeMapSrvPtr = this->create_service<mm_controller_interfaces::srv::NodeExecution>(
            "freeze", std::bind(&mm::MMNode::FreezeMap, this, std::placeholders::_1, std::placeholders::_2));
        _unfreezeMapSrvPtr = this->create_service<mm_controller_interfaces::srv::NodeExecution>(
            "unfreeze", std::bind(&mm::MMNode::UnfreezeMap, this, std::placeholders::_1, std::placeholders::_2));
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
    }
    MMNode::~MMNode()
    {
        mmClosePort();
        marvelmindAPIFree();
        RCLCPP_INFO(this->get_logger(), "Closed port connection and API link.");
    }

    bool MMNode::Start()
    {
        if (!marvelmindAPILoad())
        {
            RCLCPP_ERROR(this->get_logger(), "Marvelmind shared library API not found on system.");
            return false;
        }
        uint32_t version;
        if (mmAPIVersion(&version))
        {
            RCLCPP_INFO(this->get_logger(), "Loaded Marvelmind API version: %d", (int)version);
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to read Marvelmind API shared library.");
            return false;
        }
        // TODO:
        // API will check all devices if it its a known marvelmind device.
        // Do we need to handle specific port based on ROS configuration?
        RCLCPP_INFO(this->get_logger(), "Waiting for port...");
        int waitPort = 0;
        while (true)
        {
            if (mmOpenPort())
            {
                RCLCPP_INFO(this->get_logger(), "Port opened. Trying connection to device.");
                int waitDevice = 0;
                while (true)
                {
                    if (mmGetVersionAndId(MM_USB_DEVICE_ADDRESS, &_usbDeviceVersion))
                    {
                        RCLCPP_INFO(this->get_logger(), "Connected to " + mm::GetDeviceModelFromDeviceID(_usbDeviceVersion.fwVerDeviceType) + " device.");
                        mm::PrintDeviceInformation(this->get_logger(), _usbDeviceVersion);
                        return true;
                    }
                    rclcpp::sleep_for(std::chrono::seconds(1));
                    waitDevice++;
                    if (waitDevice > 29)
                    {
                        RCLCPP_ERROR(this->get_logger(), "Timeout reached after port opened. Failed to connect to device.");
                        return false;
                    }
                }
            }
            rclcpp::sleep_for(std::chrono::seconds(1));
            waitPort++;
            if (waitPort > 29)
            {
                RCLCPP_ERROR(this->get_logger(), "Timeout reached.. Failed to open port.");
                return false;
            }
        }
    }
112
113
114
115
116
117
118
119
120
121
122
    void MMNode::FreezeMap(const std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Request> request, std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Response> response)
    {
        if (!mmFreezeSubmap(request->id))
        {
            RCLCPP_WARN(this->get_logger(), "Failed to freeze map or submap id does not exist");
            response->is_success = false;
            response->message = "Failed to freeze map or submap id does not exist";
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Froze submap with id: %d", request->id);
levi's avatar
levi committed
123
124
125
126
127
128
129
130
131
132
133
134
            if (mmFreezeMap())
            {
                RCLCPP_INFO(this->get_logger(), "Froze map", request->id);
                response->is_success = true;
                response->message = "Froze submap with id: " + std::to_string(request->id) + ", and froze map.";
            }
            else
            {
                RCLCPP_INFO(this->get_logger(), "Failed to freeze map", request->id);
                response->is_success = false;
                response->message = "Froze submap with id: " + std::to_string(request->id) + ", but failed to freeze map";
            }
135
136
137
138
139
140
141
142
143
144
145
146
147
        }
    }
    void MMNode::UnfreezeMap(const std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Request> request, std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Response> response)
    {
        if (!mmUnfreezeSubmap(request->id))
        {
            RCLCPP_WARN(this->get_logger(), "Failed to unfreeze map or submap id does not exist");
            response->is_success = false;
            response->message = "Failed to unfreeze map or submap id does not exist";
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Unfroze submap with id: %d", request->id);
levi's avatar
levi committed
148
149
150
151
152
153
154
155
156
157
158
            if (mmUnfreezeMap())
            {
                RCLCPP_INFO(this->get_logger(), "Unfroze map", request->id);
                response->is_success = true;
                response->message = "Unfroze submap with id: " + std::to_string(request->id) + ", and unfroze the map.";
            }
            else
            {
                response->is_success = false;
                response->message = "Unfroze submap with id: " + std::to_string(request->id) + ", but failed to unfreeze map.";
            }
159
160
        }
    }
161
    // TODO: Wrapper function for sleep/wake device callback?
162
    void MMNode::WakeDeviceCallback(const std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Request> request, std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Response> response)
163
164
165
166
167
    {
        this->_statusPubTimerPtr->cancel(); //Pause timer to prevent null exception..
        //Check list on modem.
        if (this->_statusMsg.devices.empty())
        {
168
            response->is_success = false;
169
170
171
172
173
174
175
176
177
            response->message = "Devices list on modem is empty..";
            RCLCPP_WARN(this->get_logger(), "Received wake device service call, but modem list is empty.");
            this->_statusPubTimerPtr->reset(); //Start timer
            return;
        }
        unsigned int deviceFoundIndex;
        //Check parameter input, else return.
        for (size_t i = 0; i < _statusMsg.devices.size(); i++)
        {
178
            if (_statusMsg.devices[i].dev_deviceaddress == request->id)
179
180
181
182
            {
                deviceFoundIndex = i;
                break;
            }
183
            if (request->id == (int8_t)i)
184
            {
185
                response->is_success = false;
186
                response->message = "Device address not found on modem list.";
187
                RCLCPP_WARN(this->get_logger(), "Received wake device service call for device address: %d, but device is not found on modem.", request->id);
188
189
190
191
                this->_statusPubTimerPtr->reset(); //Start timer
                return;
            }
        }
192
        if (mmWakeDevice(request->id))
193
        {
194
            response->is_success = true;
195
            response->message = "Device woke from sleep..";
196
            RCLCPP_INFO(this->get_logger(), "Service request for device address: %d of type %, to wake up succeded.", request->id, _statusMsg.devices[deviceFoundIndex].dev_device_type);
197
198
199
200
201
            this->_statusPubTimerPtr->reset(); //Start timer
            return;
        }
        else
        {
202
            response->is_success = false;
203
            response->message = "Failed to wake up device.";
204
            RCLCPP_WARN(this->get_logger(), "Got service request to wake device with address %d, but it failed.", request->id);
205
206
207
208
            this->_statusPubTimerPtr->reset(); //Start timer
            return;
        }
    }
209
    void MMNode::SleepDeviceCallback(const std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Request> request, std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Response> response)
210
211
212
213
    {
        this->_statusPubTimerPtr->cancel();
        if (this->_statusMsg.devices.empty())
        {
214
            response->is_success = false;
215
216
217
218
219
220
221
222
            response->message = "Devices list on modem is empty";
            RCLCPP_WARN(this->get_logger(), "Received sleep device service call, but modem list is empty.");
            this->_statusPubTimerPtr->reset();
            return;
        }
        unsigned int deviceFoundIndex;
        for (size_t i = 0; i < _statusMsg.devices.size(); i++)
        {
223
            if (_statusMsg.devices[i].dev_deviceaddress == request->id)
224
225
226
227
            {
                deviceFoundIndex = i;
                break;
            }
228
            if (request->id == (int8_t)i)
229
            {
230
                response->is_success = false;
231
                response->message = "Deviice address not found on modem list.";
232
                RCLCPP_WARN(this->get_logger(), "Received sleep device service call for device address: %d, but device is not found on modem.", request->id);
233
234
235
                this->_statusPubTimerPtr->reset();
            }
        }
236
        if (mmSendToSleepDevice(request->id))
237
        {
238
            response->is_success = "true";
239
            response->message = "Device set to sleep..";
240
            RCLCPP_INFO(this->get_logger(), "Received sleep request service call for device address %d of type %s succeded.", request->id, _statusMsg.devices[deviceFoundIndex].dev_device_type);
241
242
243
244
245
            this->_statusPubTimerPtr->reset();
            return;
        }
        else
        {
246
            response->is_success = false;
247
            response->message = "Failed to set device to sleep";
248
            RCLCPP_WARN(this->get_logger(), "Got service request to sleep device with address %d, but it failed.", request->id);
249
250
251
252
            this->_statusPubTimerPtr->reset();
            return;
        }
    }
253
    void MMNode::RestartDeviceCallback(const std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Request> request, std::shared_ptr<mm_controller_interfaces::srv::NodeExecution::Response> response)
254
    {
levi's avatar
fixed    
levi committed
255
256
257
258
259
260
261
262
263
264
        std::string msg;
        if (!mmResetDevice(request->id))
        {
            msg = "Failed to soft reset device.";
            RCLCPP_WARN(this->get_logger(), msg);
            response->is_success = false;
            response->message = msg;
        }
        else
        {
levi's avatar
levi committed
265
            msg = "Device with id: " + std::to_string(request->id) + " successfully rebooted.";
levi's avatar
fixed    
levi committed
266
267
268
269
            RCLCPP_INFO(this->get_logger(), msg);
            response->is_success = true;
            response->message = msg;
        }
270
271
272
273
    }

    void MMNode::StatusPublisherCallback()
    {
274
        //RCLCPP_INFO(this->get_logger(), "Status callback called");
275
276
277
        if (GetDevicesListFromModem())
        {
            GetLatestLocationDataFromModem();
levi's avatar
levi committed
278
            PrintDeviceLocation(this->get_logger(),_statusMsg.devices);
279
            GetTelemetryDataFromModem();
280
            GetLatestDistances();
levi's avatar
levi committed
281
282
283
            _statusMsg.distances = FilterDistances(_statusMsg.distances);
            PrintDeviceDistances(this->get_logger(),_statusMsg.distances);
            GetSubmapSettings(0); //submap id fix?
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
        }
        _statusMsg.name = "test";
        //_statusMessagePtr->name = "test";
        this->_testCounter++;
        //_statusMessagePtr->decription = std::to_string(_testCounter);
        _statusMsg.description = std::to_string(_testCounter);
        _statusMsg.status = "New status looo";
        _statusPubPtr->publish(_statusMsg);
    }

    /* 
        Function: Get devices list from modem, update msg.
        Return: Boolean status
        Params:
    */
    bool MMNode::GetDevicesListFromModem()
    {
        MarvelmindDevicesList newList;
        if (!mmGetDevicesList(&newList))
        {
            RCLCPP_WARN(this->get_logger(), "Failed to receive devices list information, from modem.");
            return false;
        }
levi's avatar
levi committed
307
        //RCLCPP_INFO(this->get_logger(), "newList.numDevices: %d    _statusMsg.devices.size(): %d ", newList.numDevices, _statusMsg.devices.size());
308
309
310
311
312
        if (newList.numDevices == 0)
        {
            RCLCPP_WARN(this->get_logger(), "No devices configurated for map on modem.");
            return false;
        }
levi's avatar
levi committed
313
        // else
314
315
316
317
318
319
320
321
322
        // {
        //     _statusMsg.devices.clear();
        //     for (size_t i = 0; i < newList.numDevices; i++)
        //     {
        //         _statusMsg.devices.push_back(CreateDeviceStatus(&newList.devices[i]));
        //     }
        //     RCLCPP_INFO(this->get_logger(), "New device list updated.");
        //     return true;
        // }
levi's avatar
levi committed
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
        if (newList.numDevices != _statusMsg.devices.size() || _statusMsg.devices.empty())
        {
            _statusMsg.devices.clear();
            for (size_t i = 0; i < newList.numDevices; i++)
            {
                _statusMsg.devices.push_back(CreateDeviceStatus(&newList.devices[i]));
            }
            RCLCPP_INFO(this->get_logger(), "New device list updated.");
            return true;
        }
        else
        {
            for (size_t i = 0; i < newList.numDevices; i++)
            {
                _statusMsg.devices[i] = CreateDeviceStatus(&newList.devices[i]);
            }
            //RCLCPP_INFO(this->get_logger(), "Updated devices list from modem.");
            return true;
        }
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
    }
    mm_controller_interfaces::msg::DeviceStatus MMNode::CreateDeviceStatus(MarvelmindDeviceInfo *dev)
    {
        auto devStatus = mm_controller_interfaces::msg::DeviceStatus();
        devStatus.dev_deviceaddress = dev->address;
        devStatus.dev_device_type = mm::GetDeviceModelFromDeviceID(dev->fwVerDeviceType);
        devStatus.dev_firmware = mm::ComputeFirmwareString(dev->fwVerMajor, dev->fwVerMinor, dev->fwVerMinor2);
        devStatus.dev_is_duplicate_address = dev->isDuplicatedAddress;
        devStatus.dev_is_sleeping = dev->isSleeping;
        if ((dev->flags & 0x01) != 0)
            devStatus.dev_is_connected = true;
        else
            devStatus.dev_is_connected = false;
        devStatus.dev_fw_options = dev->fwOptions;
        return devStatus;
    }
    void MMNode::GetLatestLocationDataFromModem()
    {
        MarvelmindLocationsPack2 newPack;
361
        if (!mmGetLastLocations2(&newPack))
362
        {
363
364
365
366
367
368
            RCLCPP_WARN(this->get_logger(), "Failed to get locations data");
            return;
        }
        else
        {
            for (size_t i = 0; i < MM_LOCATIONS_PACK_SIZE; i++)
369
            {
370
371
                for (size_t j = 0; j < _statusMsg.devices.size(); j++)
                {
372
                    if (newPack.pos[i].address == _statusMsg.devices[j].dev_deviceaddress)
373
374
375
376
377
378
379
380
381
382
383
                    {
                        _statusMsg.devices[j].loc_x_pos_mm = newPack.pos[i].x_mm;
                        _statusMsg.devices[j].loc_y_pos_mm = newPack.pos[i].y_mm;
                        _statusMsg.devices[j].loc_z_pos_mm = newPack.pos[i].z_mm;
                        _statusMsg.devices[j].loc_status_flags = newPack.pos[i].statusFlags;
                        _statusMsg.devices[j].loc_head_index = newPack.pos[i].headIndex;
                        _statusMsg.devices[j].loc_pos_quality = newPack.pos[i].quality;
                        _statusMsg.devices[j].loc_angle_ready = newPack.pos[i].angleReady;
                        _statusMsg.devices[j].loc_angle = newPack.pos[i].angle;
                    }
                }
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
            }
        }
    }
    void MMNode::GetTelemetryDataFromModem()
    {
        MarvelmindBeaconTelemetry tel;
        for (size_t i = 0; i < _statusMsg.devices.size(); i++)
        {
            if (mmGetBeaconTelemetry(_statusMsg.devices[i].dev_deviceaddress, &tel))
            {
                _statusMsg.devices[i].tel_uptime = tel.worktimeSec;
                _statusMsg.devices[i].tel_voltage_mv = tel.voltageMv;
                _statusMsg.devices[i].tel_temperature = tel.temperature;
                _statusMsg.devices[i].tel_dbm_rssi = tel.rssi;
            }
        }
    }
401
402
403
404
405
406
407
408
    void MMNode::GetLatestDistances()
    {
        MarvelmindDistances distancesPack;
        if (!mmGetLastDistances(&distancesPack))
        {
            RCLCPP_WARN(this->get_logger(), "Failed to receive distances.");
            return;
        }
levi's avatar
levi committed
409
        _statusMsg.distances.resize(distancesPack.numDistances);
410
        for (size_t i = 0; i < distancesPack.numDistances; i++)
levi's avatar
levi committed
411
412
413
414
        { 
            _statusMsg.distances[i].dev_adr_rx = distancesPack.distance[i].addressRx;
            _statusMsg.distances[i].dev_adr_tx = distancesPack.distance[i].addressTx;
            _statusMsg.distances[i].distance_mm = distancesPack.distance[i].distance_mm;
415
416
417
418
419
        }
    }

    void MMNode::GetSubmapSettings(uint8_t submapId)
    {
levi's avatar
levi committed
420
        //RCLCPP_INFO(this->get_logger(), "Submap START");
421
422
423
424
425
426
427
428
        MarvelmindSubmapSettings submapSettings;
        if (!mmGetSubmapSettings(submapId, &submapSettings))
        {
            RCLCPP_WARN(this->get_logger(), "Failed to get submap settings.");
            return;
        }
        RCLCPP_INFO(this->get_logger(), "Starting Beacon:  %d", submapSettings.startingBeacon);
        RCLCPP_INFO(this->get_logger(), "Starting set of beacons, %d, %d, %d, %d", submapSettings.startingSet_1, submapSettings.startingSet_2, submapSettings.startingSet_3, submapSettings.startingSet_4);
429
        RCLCPP_INFO(this->get_logger(), "frozen: %d            locked: %d", submapSettings.frozen, submapSettings.locked);
levi's avatar
levi committed
430
        //RCLCPP_INFO(this->get_logger(), "Submap END");
431
    }
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    MMNode::on_activate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(this->get_logger(), "on_activated called");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    MMNode::on_cleanup(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(this->get_logger(), "on_cleanup called");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    MMNode::on_configure(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(this->get_logger(), "on_configure called");

        //Publishers setup
        std::string statusPubName = this->get_name();
        statusPubName.append("/").append(this->_paramStatusPublisherTopicName);
        _statusPubPtr = this->create_publisher<mm_controller_interfaces::msg::MMControllerStatus>(statusPubName, 10);
        _statusPubPtr->on_activate(); //Set publisher to active state
        _statusPubTimerPtr = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&MMNode::StatusPublisherCallback, this));

        std::string beaconPubName = this->get_name();
        beaconPubName.append("/").append(_paramRobotBeaconPublisherTopicName);
        _positionPubPtr = this->create_publisher<geometry_msgs::msg::Point>(beaconPubName, 10);
        _statusPubTimerPtr->is_steady();

        //Marvelmin initialisation

        //         initMarvelmindDevicesList();
        // marvelmindDevicesReadIfNeeded();
        // if (!mmGetDevicesList(&_devicesList))

        // auto test = this->get_current_state();
        // RCLCPP_INFO(this->get_logger(), test.label());
        // uint32_t version;
        // if (mmAPIVersion(&version))
        // {
        //     std::cout << version << std::endl;
        //     RCLCPP_INFO(this->get_logger(), "mm version: " + std::to_string(version));
        // }
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    MMNode::on_deactivate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(this->get_logger(), "on_deactivate called");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    MMNode::on_error(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(this->get_logger(), "on_error called");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    MMNode::on_exit(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(this->get_logger(), "on_exit called");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }
} // end namespace mm