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

added ros msg for distances

parent d80c9362
......@@ -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
)
......
......@@ -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_
......@@ -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;
}
}
......
......@@ -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)
{
......
int8 dev_adr_rx
int8 dev_adr_tx
int8 distance
\ No newline at end of file
int8 distance_mm
\ 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