added ros msg for distances

src/mm_controller/CMakeLists.txt

@@ -28,7 +28,10 @@ include_directories(
 add_library(mm-utilscpp
  src/mm_utils.cpp
 )
-ament_target_dependencies(mm-utilscpp "rclcpp_lifecycle")
+ament_target_dependencies(mm-utilscpp 
+  "rclcpp_lifecycle"
+  mm_controller_interfaces
+)
 
 
 

src/mm_controller/include/mm_controller/mm_utils.hpp

@@ -3,6 +3,7 @@
 
 #include <string.h>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include "mm_controller_interfaces/msg/mm_controller_status.hpp"
 #include <iomanip>
 extern "C"
 {
@@ -21,8 +22,11 @@ namespace mm
     std::string GetDeviceModelFromDeviceID(const uint8_t dev);
     std::string ComputeFirmwareString(const uint8_t major, const uint8_t minor, const uint8_t minor2);
     void PrintDeviceInformation(const rclcpp::Logger logger, const MarvelmindDeviceVersion dev);
+    void PrintDeviceDistances(const rclcpp::Logger logger, const std::vector<mm_controller_interfaces::msg::DeviceDistance> distances);
+    void PrintDeviceLocation(const rclcpp::Logger logger, const std::vector<mm_controller_interfaces::msg::DeviceStatus> devices);
     template <typename T>
     std::string int_to_hex(T i);
     std::string GenerateExecutionResult(bool executionSucces, std::string msg);
+    std::vector<mm_controller_interfaces::msg::DeviceDistance> FilterDistances(const std::vector<mm_controller_interfaces::msg::DeviceDistance> distances);
 }
 #endif // __MM_UTILS_HPP_

src/mm_controller/src/mm_node.cpp

@@ -275,18 +275,12 @@ namespace mm
         if (GetDevicesListFromModem())
         {
             GetLatestLocationDataFromModem();
+            PrintDeviceLocation(this->get_logger(),_statusMsg.devices);
             GetTelemetryDataFromModem();
             GetLatestDistances();
-            GetSubmapSettings(0); //submap id
-            for (size_t i = 0; i < _statusMsg.devices.size(); i++)
-            {
-
-                auto test = _statusMsg.devices[i];
-                if (!test.dev_is_sleeping)
-                {
-                    RCLCPP_INFO(this->get_logger(), "devAddr:%d    X: %d Y: %d", test.dev_deviceaddress, test.loc_x_pos_mm, test.loc_y_pos_mm);
-                }
-            }
+            _statusMsg.distances = FilterDistances(_statusMsg.distances);
+            PrintDeviceDistances(this->get_logger(),_statusMsg.distances);
+            GetSubmapSettings(0); //submap id fix?
         }
         _statusMsg.name = "test";
         //_statusMessagePtr->name = "test";
@@ -407,27 +401,17 @@ namespace mm
     void MMNode::GetLatestDistances()
     {
         MarvelmindDistances distancesPack;
-        // for (size_t i = 2; i < 6; i++)
-        // {
-        //     for (size_t j = 2; i < 6; i++)
-        //     {
-        //         if (i == j)
-        //             continue;
-        //         distancesPack.distance[i].addressTx = i;
-        //         distancesPack.distance[i].addressRx = j;
-        //     }
-        // }
         if (!mmGetLastDistances(&distancesPack))
         {
             RCLCPP_WARN(this->get_logger(), "Failed to receive distances.");
             return;
         }
+        _statusMsg.distances.resize(distancesPack.numDistances);
         for (size_t i = 0; i < distancesPack.numDistances; i++)
-        {
-            RCLCPP_INFO(this->get_logger(), "AddrRx: %d AddrTx: %d Dist: %d",
-                        distancesPack.distance[i].addressRx,
-                        distancesPack.distance[i].addressTx,
-                        distancesPack.distance[i].distance_mm);
+        { 
+            _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;
         }
     }
 

src/mm_controller/src/mm_utils.cpp

@@ -55,6 +55,35 @@ namespace mm
         RCLCPP_INFO(logger, "CPU ID: " + mm::int_to_hex(dev.cpuId));
     }
 
+    void PrintDeviceDistances(const rclcpp::Logger logger, const std::vector<mm_controller_interfaces::msg::DeviceDistance> distances)
+    {
+        for (size_t i = 0; i < distances.size(); i++)
+        {
+            RCLCPP_INFO(logger, "AddrRx: %d AddrTx: %d Distance: %dmm", distances[i].dev_adr_rx, distances[i].dev_adr_tx, distances[i].distance_mm);
+        }
+    }
+    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;
+    }
+
     template <typename T>
     std::string int_to_hex(T i)
     {

src/mm_controller_interfaces/msg/DeviceDistance.msg

 int8 dev_adr_rx
 int8 dev_adr_tx
-int8 distance
+int8 distance_mm
\ No newline at end of file