Connect up to ros.
This commit is contained in:
1 parent
f5adbb59a4
commit
7ec72dbbdb
8 files changed
+529
No files matched your search
@@ -1,3 +1,13 @@
|
||||
//
|
||||
// Copyright 2026 James Pace
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
|
||||
//
|
||||
// This Source Code Form is "Incompatible With Secondary Licenses", as
|
||||
// defined by the Mozilla Public License, v. 2.0.
|
||||
//
|
||||
#include <j7s_diagnostics_ros/conversions.hpp>
|
||||
|
||||
std::string convertString(const rust::String & rust)
|
||||
@@ -100,6 +110,19 @@ rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> fromMsg(
|
||||
return toReturn;
|
||||
}
|
||||
|
||||
rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> fromMsg(
|
||||
const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & msgs)
|
||||
{
|
||||
rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> toReturn;
|
||||
toReturn.reserve(msgs.size());
|
||||
|
||||
for (const auto & rosMsg : msgs)
|
||||
{
|
||||
toReturn.emplace_back(fromMsg(rosMsg));
|
||||
}
|
||||
return toReturn;
|
||||
}
|
||||
|
||||
diagnostic_msgs::msg::DiagnosticStatus toMsg(const j7s_diagnostics_cxx::DiagnosticStatus & status)
|
||||
{
|
||||
diagnostic_msgs::msg::DiagnosticStatus toReturn;
|
||||
|
||||
@@ -0,0 +1,60 @@
|
||||
//
|
||||
// Copyright 2026 James Pace
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
|
||||
//
|
||||
// This Source Code Form is "Incompatible With Secondary Licenses", as
|
||||
// defined by the Mozilla Public License, v. 2.0.
|
||||
//
|
||||
#include <chrono>
|
||||
#include <j7s_diagnostics_ros/conversions.hpp>
|
||||
#include <j7s_diagnostics_ros/j7s_diagnostic_node.hpp>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace j7s
|
||||
{
|
||||
DiagnosticAggregator::DiagnosticAggregator(const rclcpp::NodeOptions & options) :
|
||||
Node("j7s_diagnostic_agg", options)
|
||||
{
|
||||
m_publisher =
|
||||
this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics_agg", 1);
|
||||
m_subscriber = this->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
|
||||
"/diagnostics", 1,
|
||||
[this](const diagnostic_msgs::msg::DiagnosticArray & msg)
|
||||
{
|
||||
for (const auto status : msg.status)
|
||||
{
|
||||
m_queue.emplace_back(status);
|
||||
}
|
||||
});
|
||||
m_timer = this->create_timer(
|
||||
1s,
|
||||
[this]()
|
||||
{
|
||||
const auto queueCopy = m_queue;
|
||||
m_queue.clear();
|
||||
|
||||
auto tree = j7s_diagnostics_cxx::DiagnosticTree::build();
|
||||
tree->add_statuses(fromMsg(queueCopy));
|
||||
tree->reconcile();
|
||||
const auto aggregate = toMsg(tree->aggregate());
|
||||
m_publisher->publish(aggregate);
|
||||
});
|
||||
}
|
||||
}; // namespace j7s
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = std::make_shared<j7s::DiagnosticAggregator>(rclcpp::NodeOptions());
|
||||
|
||||
rclcpp::spin(node);
|
||||
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,3 +1,13 @@
|
||||
//
|
||||
// Copyright 2026 James Pace
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla Public
|
||||
// License, v. 2.0. If a copy of the MPL was not distributed with this
|
||||
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
|
||||
//
|
||||
// This Source Code Form is "Incompatible With Secondary Licenses", as
|
||||
// defined by the Mozilla Public License, v. 2.0.
|
||||
//
|
||||
#include <fmt/core.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
Reference in new issue
Block a user