127 lines
3.9 KiB
C++
127 lines
3.9 KiB
C++
#include <j7s_diagnostics_ros/conversions.hpp>
|
|
|
|
std::string convertString(const rust::String & rust)
|
|
{
|
|
auto copy = rust;
|
|
return std::string(copy.c_str());
|
|
}
|
|
|
|
j7s_diagnostics_cxx::DiagnosticLevel convertLevel(const uint8_t rosLevel)
|
|
{
|
|
switch (rosLevel)
|
|
{
|
|
case diagnostic_msgs::msg::DiagnosticStatus::OK:
|
|
return j7s_diagnostics_cxx::DiagnosticLevel::OK;
|
|
case diagnostic_msgs::msg::DiagnosticStatus::WARN:
|
|
return j7s_diagnostics_cxx::DiagnosticLevel::WARN;
|
|
case diagnostic_msgs::msg::DiagnosticStatus::ERROR:
|
|
return j7s_diagnostics_cxx::DiagnosticLevel::ERROR;
|
|
case diagnostic_msgs::msg::DiagnosticStatus::STALE:
|
|
default:
|
|
return j7s_diagnostics_cxx::DiagnosticLevel::STALE;
|
|
}
|
|
}
|
|
|
|
uint8_t convertLevel(const j7s_diagnostics_cxx::DiagnosticLevel & level)
|
|
{
|
|
switch (level)
|
|
{
|
|
case j7s_diagnostics_cxx::DiagnosticLevel::OK:
|
|
return diagnostic_msgs::msg::DiagnosticStatus::OK;
|
|
case j7s_diagnostics_cxx::DiagnosticLevel::WARN:
|
|
return diagnostic_msgs::msg::DiagnosticStatus::WARN;
|
|
case j7s_diagnostics_cxx::DiagnosticLevel::ERROR:
|
|
return diagnostic_msgs::msg::DiagnosticStatus::ERROR;
|
|
case j7s_diagnostics_cxx::DiagnosticLevel::STALE:
|
|
case j7s_diagnostics_cxx::DiagnosticLevel::UNSET:
|
|
default:
|
|
return diagnostic_msgs::msg::DiagnosticStatus::STALE;
|
|
}
|
|
}
|
|
|
|
std::vector<diagnostic_msgs::msg::KeyValue> convertValues(
|
|
const rust::Box<j7s_diagnostics_cxx::StringMap> & map)
|
|
{
|
|
std::vector<diagnostic_msgs::msg::KeyValue> toReturn;
|
|
|
|
const rust::Vec<rust::String> keys = map->keys();
|
|
if (keys.empty())
|
|
{
|
|
return toReturn;
|
|
}
|
|
|
|
for (const auto & key : keys)
|
|
{
|
|
const auto value = map->get(key);
|
|
diagnostic_msgs::msg::KeyValue msg;
|
|
msg.key = convertString(key);
|
|
msg.value = std::string(value->value().c_str());
|
|
toReturn.emplace_back(msg);
|
|
}
|
|
|
|
return toReturn;
|
|
}
|
|
|
|
rust::Box<j7s_diagnostics_cxx::StringMap> convertValues(
|
|
const std::vector<diagnostic_msgs::msg::KeyValue> & map)
|
|
{
|
|
rust::Box<j7s_diagnostics_cxx::StringMap> toReturn = j7s_diagnostics_cxx::StringMap::build();
|
|
|
|
for (const auto & pair : map)
|
|
{
|
|
toReturn->insert(rust::String(pair.key), rust::String(pair.value));
|
|
}
|
|
|
|
return toReturn;
|
|
}
|
|
|
|
j7s_diagnostics_cxx::DiagnosticStatus fromMsg(const diagnostic_msgs::msg::DiagnosticStatus & msg)
|
|
{
|
|
auto toReturn = j7s_diagnostics_cxx::DiagnosticStatus::build();
|
|
toReturn.level = convertLevel(msg.level);
|
|
toReturn.name = rust::String(msg.name);
|
|
toReturn.message = rust::String(msg.message);
|
|
toReturn.hardware_id = rust::String(msg.hardware_id);
|
|
toReturn.values = convertValues(msg.values);
|
|
|
|
return toReturn;
|
|
}
|
|
|
|
rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> fromMsg(
|
|
const diagnostic_msgs::msg::DiagnosticArray & msg)
|
|
{
|
|
rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> toReturn;
|
|
toReturn.reserve(msg.status.size());
|
|
|
|
for (const auto & rosMsg : msg.status)
|
|
{
|
|
toReturn.emplace_back(fromMsg(rosMsg));
|
|
}
|
|
return toReturn;
|
|
}
|
|
|
|
diagnostic_msgs::msg::DiagnosticStatus toMsg(const j7s_diagnostics_cxx::DiagnosticStatus & status)
|
|
{
|
|
diagnostic_msgs::msg::DiagnosticStatus toReturn;
|
|
toReturn.level = convertLevel(status.level);
|
|
toReturn.name = convertString(status.name);
|
|
toReturn.message = convertString(status.message);
|
|
toReturn.hardware_id = convertString(status.hardware_id);
|
|
toReturn.values = convertValues(status.values);
|
|
|
|
return toReturn;
|
|
}
|
|
|
|
diagnostic_msgs::msg::DiagnosticArray toMsg(
|
|
const rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> & statuses)
|
|
{
|
|
diagnostic_msgs::msg::DiagnosticArray msg;
|
|
msg.status.reserve(statuses.size());
|
|
|
|
for (const auto & status : statuses)
|
|
{
|
|
msg.status.emplace_back(toMsg(status));
|
|
}
|
|
return msg;
|
|
}
|