Commit e13c0111 authored by levi's avatar levi
Browse files

rdy for test

parent 26721946
......@@ -275,11 +275,11 @@ namespace mm
if (GetDevicesListFromModem())
{
GetLatestLocationDataFromModem();
PrintDeviceLocation(this->get_logger(),_statusMsg.devices);
PrintDeviceLocation(this->get_logger(), _statusMsg.devices);
GetTelemetryDataFromModem();
GetLatestDistances();
_statusMsg.distances = FilterDistances(_statusMsg.distances);
PrintDeviceDistances(this->get_logger(),_statusMsg.distances);
//_statusMsg.distances = FilterDistances(_statusMsg.distances);
//PrintDeviceDistances(this->get_logger(), _statusMsg.distances);
GetSubmapSettings(0); //submap id fix?
}
_statusMsg.name = "test";
......@@ -406,13 +406,30 @@ namespace mm
RCLCPP_WARN(this->get_logger(), "Failed to receive distances.");
return;
}
_statusMsg.distances.resize(distancesPack.numDistances);
// _statusMsg.distances = std::vector<mm_controller_interfaces::msg::DeviceDistance>();
for (size_t i = 0; i < distancesPack.numDistances; i++)
{
_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;
{
RCLCPP_INFO(this->get_logger(), "AddrRx: %d AddrTx: %d Dist: %d",
distancesPack.distance[i].addressRx,
distancesPack.distance[i].addressTx,
distancesPack.distance[i].distance_mm);
}
// for (size_t i = 0; i < distancesPack.numDistances; i++)
// {
// _statusMsg.distances.clear();
// auto distance = mm_controller_interfaces::msg::DeviceDistance();
// //mm_controller_interfaces::msg::DeviceDistance distance;
// distance.dev_adr_rx = distancesPack.distance[i].addressRx;
// distance.dev_adr_tx = distancesPack.distance[i].addressTx;
// distance.distance_mm = distancesPack.distance[i].distance_mm;
// _statusMsg.distances.push_back(distance);
// // _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;
// }
// RCLCPP_INFO(this->get_logger(),"statusRX %d packRX %d",_statusMsg.distances[0].distance_mm,distancesPack.distance[0].distance_mm);
}
void MMNode::GetSubmapSettings(uint8_t submapId)
......
int8 dev_adr_rx
int8 dev_adr_tx
int8 distance_mm
\ No newline at end of file
uint8 dev_adr_rx
uint8 dev_adr_tx
uint8 distance_mm
\ No newline at end of file
Markdown is supported
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