Commit 4601424c authored by levi's avatar levi
Browse files

added ros msg for distances

parent d80c9362
...@@ -28,7 +28,10 @@ include_directories( ...@@ -28,7 +28,10 @@ include_directories(
add_library(mm-utilscpp add_library(mm-utilscpp
src/mm_utils.cpp src/mm_utils.cpp
) )
ament_target_dependencies(mm-utilscpp "rclcpp_lifecycle") ament_target_dependencies(mm-utilscpp
"rclcpp_lifecycle"
mm_controller_interfaces
)
......
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include <string.h> #include <string.h>
#include <rclcpp_lifecycle/lifecycle_node.hpp> #include <rclcpp_lifecycle/lifecycle_node.hpp>
#include "mm_controller_interfaces/msg/mm_controller_status.hpp"
#include <iomanip> #include <iomanip>
extern "C" extern "C"
{ {
...@@ -21,8 +22,11 @@ namespace mm ...@@ -21,8 +22,11 @@ namespace mm
std::string GetDeviceModelFromDeviceID(const uint8_t dev); std::string GetDeviceModelFromDeviceID(const uint8_t dev);
std::string ComputeFirmwareString(const uint8_t major, const uint8_t minor, const uint8_t minor2); 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 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> template <typename T>
std::string int_to_hex(T i); std::string int_to_hex(T i);
std::string GenerateExecutionResult(bool executionSucces, std::string msg); 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_ #endif // __MM_UTILS_HPP_
...@@ -275,18 +275,12 @@ namespace mm ...@@ -275,18 +275,12 @@ namespace mm
if (GetDevicesListFromModem()) if (GetDevicesListFromModem())
{ {
GetLatestLocationDataFromModem(); GetLatestLocationDataFromModem();
PrintDeviceLocation(this->get_logger(),_statusMsg.devices);
GetTelemetryDataFromModem(); GetTelemetryDataFromModem();
GetLatestDistances(); GetLatestDistances();
GetSubmapSettings(0); //submap id _statusMsg.distances = FilterDistances(_statusMsg.distances);
for (size_t i = 0; i < _statusMsg.devices.size(); i++) PrintDeviceDistances(this->get_logger(),_statusMsg.distances);
{ GetSubmapSettings(0); //submap id fix?
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.name = "test"; _statusMsg.name = "test";
//_statusMessagePtr->name = "test"; //_statusMessagePtr->name = "test";
...@@ -407,27 +401,17 @@ namespace mm ...@@ -407,27 +401,17 @@ namespace mm
void MMNode::GetLatestDistances() void MMNode::GetLatestDistances()
{ {
MarvelmindDistances distancesPack; 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)) if (!mmGetLastDistances(&distancesPack))
{ {
RCLCPP_WARN(this->get_logger(), "Failed to receive distances."); RCLCPP_WARN(this->get_logger(), "Failed to receive distances.");
return; return;
} }
_statusMsg.distances.resize(distancesPack.numDistances);
for (size_t i = 0; i < distancesPack.numDistances; i++) for (size_t i = 0; i < distancesPack.numDistances; i++)
{ {
RCLCPP_INFO(this->get_logger(), "AddrRx: %d AddrTx: %d Dist: %d", _statusMsg.distances[i].dev_adr_rx = distancesPack.distance[i].addressRx;
distancesPack.distance[i].addressRx, _statusMsg.distances[i].dev_adr_tx = distancesPack.distance[i].addressTx;
distancesPack.distance[i].addressTx, _statusMsg.distances[i].distance_mm = distancesPack.distance[i].distance_mm;
distancesPack.distance[i].distance_mm);
} }
} }
......
...@@ -55,6 +55,35 @@ namespace mm ...@@ -55,6 +55,35 @@ namespace mm
RCLCPP_INFO(logger, "CPU ID: " + mm::int_to_hex(dev.cpuId)); 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> template <typename T>
std::string int_to_hex(T i) std::string int_to_hex(T i)
{ {
......
int8 dev_adr_rx int8 dev_adr_rx
int8 dev_adr_tx int8 dev_adr_tx
int8 distance int8 distance_mm
\ No newline at end of file \ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment