Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Feature/diagnostics (#135)
Browse files Browse the repository at this point in the history
* Initial port of diagnostics to C++

* Remap axis camera topics to match API

* Monitor MCU Status message frequency

* Added firmware version check

* Group MCU diagnostics together

* Improve messaging around firmware versions

* Disable MCU diagnostics for A200
hilary-luo authored Jan 28, 2025
1 parent 037ac37 commit 1394b88
Showing 12 changed files with 527 additions and 359 deletions.
28 changes: 23 additions & 5 deletions clearpath_diagnostics/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -7,14 +7,32 @@ endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(clearpath_platform_msgs REQUIRED)
find_package(diagnostic_updater REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)

ament_python_install_package(${PROJECT_NAME})
set(DEPENDENCIES
ament_cmake
clearpath_platform_msgs
diagnostic_updater
rclcpp
sensor_msgs
)

install(PROGRAMS
${PROJECT_NAME}/diagnostics_updater
DESTINATION lib/${PROJECT_NAME}
add_executable(clearpath_diagnostic_updater
src/clearpath_diagnostic_updater.cpp
)
target_include_directories(clearpath_diagnostic_updater PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
target_compile_features(clearpath_diagnostic_updater PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17

ament_target_dependencies(clearpath_diagnostic_updater ${DEPENDENCIES})
target_link_libraries(clearpath_diagnostic_updater)

install(TARGETS clearpath_diagnostic_updater
DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY config launch
DESTINATION share/${PROJECT_NAME}
Empty file.
249 changes: 0 additions & 249 deletions clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater

This file was deleted.

35 changes: 35 additions & 0 deletions clearpath_diagnostics/config/diagnostic_aggregator.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
diagnostic_aggregator:
ros__parameters:
path: Clearpath Diagnostics
platform:
type: diagnostic_aggregator/AnalyzerGroup
path: Platform
analyzers:
mcu:
type: diagnostic_aggregator/GenericAnalyzer
path: MCU
contains: [ 'MCU' ]
sensors:
type: diagnostic_aggregator/AnalyzerGroup
path: Sensors
analyzers:
cameras:
type: diagnostic_aggregator/GenericAnalyzer
path: Cameras
contains: [ 'camera' ]
lidar2d:
type: diagnostic_aggregator/GenericAnalyzer
path: Lidar2D
contains: [ 'lidar2d' ]
lidar3d:
type: diagnostic_aggregator/GenericAnalyzer
path: Lidar3D
contains: [ 'lidar3d' ]
imu:
type: diagnostic_aggregator/GenericAnalyzer
path: IMU
contains: [ 'imu' ]
gps:
type: diagnostic_aggregator/GenericAnalyzer
path: GPS
contains: [ 'gps' ]
8 changes: 8 additions & 0 deletions clearpath_diagnostics/config/diagnostic_updater.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
clearpath_diagnostic_updater:
ros__parameters:
serial_number: unknown
platform_model: unknown
ros_distro: unknown
latest_apt_firmware_version: unknown
installed_apt_firmware_version: unknown
topics: {}
36 changes: 0 additions & 36 deletions clearpath_diagnostics/config/diagnostics.yaml

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
/**
Software License Agreement (BSD)
\file clearpath_diagnostic_updater.hpp
\authors Hilary Luo <hluo@clearpathrobotics.com>
\copyright Copyright (c) 2025, Clearpath Robotics, Inc., All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
following disclaimer in the documentation and/or other materials provided with the distribution.
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef CLEARPATH_DIAGNOSTIC_UPDATER_HPP
#define CLEARPATH_DIAGNOSTIC_UPDATER_HPP

#include <map>
#include <list>

#include "diagnostic_updater/diagnostic_updater.hpp"
#include "diagnostic_updater/publisher.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/magnetic_field.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

#include "clearpath_platform_msgs/msg/status.hpp"

namespace clearpath
{

class ClearpathDiagnosticUpdater : public rclcpp::Node
{
public:
ClearpathDiagnosticUpdater();

private:
std::string serial_number_;
std::string platform_model_;
std::string namespace_;

// MCU Status Info
std::string ros_distro_; // Specifically the ros distro used for the firmware apt package check
std::string latest_apt_firmware_version_;
std::string installed_apt_firmware_version_;
std::string mcu_firmware_version_;
std::string mcu_platform_model_;
double mcu_status_rate_;
int mcu_temperature_;
int pcb_temperature_;
long connection_uptime_;
long mcu_uptime_;
std::shared_ptr<diagnostic_updater::FrequencyStatus> mcu_freq_status_;
rclcpp::Subscription<clearpath_platform_msgs::msg::Status>::SharedPtr sub_mcu_status_;

diagnostic_updater::Updater updater_;
std::map<std::string, std::map<std::string, rclcpp::Parameter>> topic_map_;

// Lists to ensure all variables relating to the rate monitoring persist until spin
std::list<double> rates_;
std::list<std::shared_ptr<diagnostic_updater::HeaderlessTopicDiagnostic>> topic_diagnostics_;
std::list<std::shared_ptr<void>> subscriptions_;

void mcu_callback(const clearpath_platform_msgs::msg::Status & msg);
void check_firmware_version(diagnostic_updater::DiagnosticStatusWrapper & stat);
void mcu_status_diagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat);

std::string get_mandatory_param(std::string param_name);
void setup_topic_rate_diagnostics();

template<class MsgType> void add_rate_diagnostic(const std::string topic_name, const double rate)
{
// Store the rate so that it can be accessed via a pointer and is not deleted
rates_.push_back(rate);

// Create the diagnostic task object that handles calculating and publishing rate statistics
auto topic_diagnostic =
std::make_shared<diagnostic_updater::HeaderlessTopicDiagnostic>(
topic_name,
updater_,
diagnostic_updater::FrequencyStatusParam(&rates_.back(), &rates_.back(), 0.1, 5));

// Store the diagnostic task object so that it can be accessed via a pointer and is not deleted
topic_diagnostics_.push_back(topic_diagnostic);

auto sub = this->create_subscription<MsgType>(
topic_name,
rclcpp::SensorDataQoS(),
[this, topic_diagnostic]
([[maybe_unused]] const MsgType & msg) {
topic_diagnostic->tick();
});
subscriptions_.push_back(std::static_pointer_cast<void>(sub));
}
};

}

#endif // CLEARPATH_DIAGNOSTIC_UPDATER_HPP
101 changes: 44 additions & 57 deletions clearpath_diagnostics/launch/diagnostics.launch.py
Original file line number Diff line number Diff line change
@@ -1,9 +1,8 @@
#!/usr/bin/env python3

# Software License Agreement (BSD)
#
# @author Roni Kreinin <rkreinin@clearpathrobotics.com>
# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved.
# @author Hilary Luo <hluo@clearpathrobotics.com>
# @copyright (c) 2025, Clearpath Robotics, Inc., All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
@@ -28,63 +27,50 @@
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

# Redistribution and use in source and binary forms, with or without
# modification, is not permitted without the express permission
# of Clearpath Robotics.

from ament_index_python.packages import get_package_share_directory

from clearpath_config.clearpath_config import ClearpathConfig
from clearpath_config.common.utils.yaml import read_yaml

from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
GroupAction,
OpaqueFunction,
)
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution

from launch_ros.actions import Node, PushRosNamespace


ARGUMENTS = [
DeclareLaunchArgument(
'setup_path',
default_value='/etc/clearpath/',
description='Clearpath setup path',
)
]


def launch_setup(context, *args, **kwargs):
pkg_clearpath_diagnostics = get_package_share_directory('clearpath_diagnostics')
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

setup_path = LaunchConfiguration('setup_path')

robot_yaml = PathJoinSubstitution(
[setup_path, 'robot.yaml']
)

analyzer_params_filepath = PathJoinSubstitution(
[pkg_clearpath_diagnostics, 'config', 'diagnostics.yaml']
)

# Read robot YAML
config = read_yaml(robot_yaml.perform(context))
# Parse robot YAML into config
clearpath_config = ClearpathConfig(config)

namespace = clearpath_config.system.namespace
diagnostics = GroupAction(
def generate_launch_description():
namespace = LaunchConfiguration('namespace')
aggregator_parameters = LaunchConfiguration('aggregator_parameters')
updater_parameters = LaunchConfiguration('updater_parameters')

arg_namespace = DeclareLaunchArgument(
'namespace',
default_value='',
description='Robot namespace, applied to diagnostic nodes and topics')

arg_aggregator_params = DeclareLaunchArgument(
'aggregator_parameters',
default_value=PathJoinSubstitution([
FindPackageShare('clearpath_diagnostics'),
'config',
'diagnostic_aggregator.yaml'
]))

arg_updater_params = DeclareLaunchArgument(
'updater_parameters',
default_value=PathJoinSubstitution([
FindPackageShare('clearpath_diagnostics'),
'config',
'diagnostic_updater.yaml'
]))

diagnostics_action = GroupAction(
[
PushRosNamespace(namespace),
# Aggregator
Node(
package='diagnostic_aggregator',
executable='aggregator_node',
name='diagnostic_aggregator',
namespace=namespace,
output='screen',
parameters=[analyzer_params_filepath],
parameters=[aggregator_parameters],
remappings=[
('/diagnostics', 'diagnostics'),
('/diagnostics_agg', 'diagnostics_agg'),
@@ -94,22 +80,23 @@ def launch_setup(context, *args, **kwargs):
# Updater
Node(
package='clearpath_diagnostics',
executable='diagnostics_updater',
executable='clearpath_diagnostic_updater',
name='clearpath_diagnostic_updater',
namespace=namespace,
output='screen',
parameters=[updater_parameters],
remappings=[
('/diagnostics', 'diagnostics'),
('/diagnostics_agg', 'diagnostics_agg'),
('/diagnostics_toplevel_state', 'diagnostics_toplevel_state'),
],
arguments=['-s', setup_path]
),
]
)

return [diagnostics]


def generate_launch_description():
ld = LaunchDescription(ARGUMENTS)
ld.add_action(OpaqueFunction(function=launch_setup))
ld = LaunchDescription()
ld.add_action(arg_namespace)
ld.add_action(arg_aggregator_params)
ld.add_action(arg_updater_params)
ld.add_action(diagnostics_action)
return ld
21 changes: 12 additions & 9 deletions clearpath_diagnostics/package.xml
Original file line number Diff line number Diff line change
@@ -3,20 +3,23 @@
<package format="3">
<name>clearpath_diagnostics</name>
<version>1.1.0</version>
<description>Clearpath Robot Diagnostics scripts</description>
<maintainer email="rkreinin@clearpathrobotics.com">rkreinin</maintainer>
<description>Clearpath Robot Diagnostics Monitor</description>

<maintainer email="hluo@clearpathrobotics.com">Hilary Luo</maintainer>
<license>BSD</license>

<author email="rkreinin@clearpathrobotics.com">Roni Kreinin</author>
<author email="hluo@clearpathrobotics.com">Hilary Luo</author>

<buildtool_depend>ament_cmake</buildtool_depend>

<exec_depend version_gte="1.0.0">clearpath_config</exec_depend>
<exec_depend version_gte="1.0.0">clearpath_generator_common</exec_depend>
<exec_depend version_gte="1.0.1">clearpath_platform_msgs</exec_depend>
<exec_depend>diagnostic_updater</exec_depend>
<depend version_gte="1.0.1">clearpath_platform_msgs</depend>
<depend>diagnostic_updater</depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>

<exec_depend>diagnostic_aggregator</exec_depend>
<exec_depend>diagnostic_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>ros2launch</exec_depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
266 changes: 266 additions & 0 deletions clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,266 @@
/**
Software License Agreement (BSD)
\file clearpath_diagnostic_updater.cpp
\authors Hilary Luo <hluo@clearpathrobotics.com>
\copyright Copyright (c) 2025, Clearpath Robotics, Inc., All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
following disclaimer in the documentation and/or other materials provided with the distribution.
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "clearpath_diagnostics/clearpath_diagnostic_updater.hpp"

#define UNKNOWN "unknown"

namespace clearpath
{

ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater()
: Node(
"clearpath_diagnostic_updater",
rclcpp::NodeOptions().allow_undeclared_parameters(true).
automatically_declare_parameters_from_overrides(true)),
updater_(this) // Create the diagnostic updater object
{
serial_number_ = this->get_mandatory_param("serial_number");
platform_model_ = this->get_mandatory_param("platform_model");
ros_distro_ = this->get_mandatory_param("ros_distro");
latest_apt_firmware_version_ = this->get_mandatory_param("latest_apt_firmware_version");
installed_apt_firmware_version_ = this->get_mandatory_param("installed_apt_firmware_version");

// Initialize variables that are populated in callbacks
mcu_firmware_version_ = UNKNOWN;
mcu_platform_model_ = UNKNOWN;

// Set Hardware ID as serial number in diagnostics
updater_.setHardwareID(serial_number_);
if (latest_apt_firmware_version_ == "not_applicable") {
RCLCPP_INFO(this->get_logger(), "No MCU indicated, MCU diagnostics disabled.");
} else if (latest_apt_firmware_version_ != "simulated") {
// Publish MCU Status information as diagnostics
updater_.add("MCU Status", this, &ClearpathDiagnosticUpdater::mcu_status_diagnostic);
updater_.add("MCU Firmware Version", this, &ClearpathDiagnosticUpdater::check_firmware_version);
RCLCPP_INFO(this->get_logger(), "MCU diagnostics started.");
}

mcu_status_rate_ = 1.0;
mcu_freq_status_ = std::make_shared<diagnostic_updater::FrequencyStatus>(
diagnostic_updater::FrequencyStatusParam(&mcu_status_rate_, &mcu_status_rate_, 0.1, 5));

// subscribe to MCU status
sub_mcu_status_ =
this->create_subscription<clearpath_platform_msgs::msg::Status>(
"platform/mcu/status",
rclcpp::SensorDataQoS(),
std::bind(&ClearpathDiagnosticUpdater::mcu_callback, this, std::placeholders::_1));

setup_topic_rate_diagnostics();
}

// get parameter from yaml and log an error if the parameter is not present
std::string ClearpathDiagnosticUpdater::get_mandatory_param(std::string param_name)
{
try {
return this->get_parameter(param_name).as_string();
} catch (const std::exception & e) {
RCLCPP_ERROR(this->get_logger(), "Could not retrieve parameter %s: %s",
param_name.c_str(), e.what());
return UNKNOWN;
}
}

void ClearpathDiagnosticUpdater::check_firmware_version(
diagnostic_updater::DiagnosticStatusWrapper & stat)
{
if (latest_apt_firmware_version_ == "not_found") {
stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::ERROR,
"ros-%s-clearpath-firmware package not found",
ros_distro_.c_str());
} else if (latest_apt_firmware_version_ == UNKNOWN) {
stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::ERROR,
"ros-%s-clearpath-firmware package version not provided in config",
ros_distro_.c_str());
} else if (mcu_firmware_version_ == UNKNOWN) {
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR,
"No firmware version received from MCU");
} else if (mcu_firmware_version_ == latest_apt_firmware_version_) {
stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::OK,
"Firmware is up to date (%s)",
mcu_firmware_version_.c_str());
} else if (mcu_firmware_version_ < latest_apt_firmware_version_) {
stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::WARN,
"New firmware available: (%s) -> (%s)",
mcu_firmware_version_.c_str(),
latest_apt_firmware_version_.c_str());
} else {
stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::WARN,
"ros-%s-clearpath-firmware package is outdated",
ros_distro_.c_str());
}
stat.add("Latest Firmware Version Package", latest_apt_firmware_version_);
stat.add("Firmware Version Installed on Computer", installed_apt_firmware_version_);
stat.add("Firmware Version on MCU", mcu_firmware_version_);
}

// save data from MCU Status messages
void ClearpathDiagnosticUpdater::mcu_callback(const clearpath_platform_msgs::msg::Status & msg)
{
mcu_firmware_version_ = msg.firmware_version;
mcu_platform_model_ = msg.hardware_id;
mcu_uptime_ = msg.mcu_uptime.sec;
connection_uptime_ = msg.connection_uptime.sec;
mcu_temperature_ = msg.mcu_temperature;
pcb_temperature_ = msg.pcb_temperature;
mcu_freq_status_->tick();
}

void ClearpathDiagnosticUpdater::mcu_status_diagnostic(
diagnostic_updater::DiagnosticStatusWrapper & stat)
{
stat.add("Firmware Version", mcu_firmware_version_);
stat.add("Platform Model", mcu_platform_model_);
stat.add("MCU Uptime", mcu_uptime_);
stat.add("Connection Uptime", connection_uptime_);
stat.add("MCU Temperature", mcu_temperature_);
stat.add("PCB Temperature", pcb_temperature_);

mcu_freq_status_->run(stat);
}

void ClearpathDiagnosticUpdater::setup_topic_rate_diagnostics()
{
std::map<std::string, rclcpp::Parameter> topic_map_raw;

// Get all parameters under the "topics" key in the yaml and store it in map format
if (!this->get_parameters("topics", topic_map_raw)) {
RCLCPP_WARN(this->get_logger(), "No topics found to monitor.");
return;
}
RCLCPP_INFO(this->get_logger(), "Retrieved %zu topic parameters.", topic_map_raw.size());

// Parse the raw topic map into a map of parameters per topic, stored in another map with
// the topic as the key
for (const auto & entry : topic_map_raw) {

// Identify the topic name and parameter name in the entry key
auto key = entry.first; // Contains topic_name.param_name
auto pos = key.find(".");
auto topic_name = key.substr(0, pos);
auto param_name = key.substr(pos + 1);

// RCLCPP_INFO(this->get_logger(),
// "Diagnostics config: Topic is %s, parameter %s = %s.",
// topic_name.c_str(), param_name.c_str(), entry.second.value_to_string().c_str());

// Add the parameter to the topic specific map
if (auto it = topic_map_.find(topic_name); it != topic_map_.end()) {
// Topic already exists as a key
it->second[param_name] = entry.second;
} else {
// Topic needs to be added as a new key
std::map<std::string, rclcpp::Parameter> map;
map[param_name] = entry.second;
topic_map_[topic_name] = map;
}
}

// For each topic, create a subscription that monitors the publishing frequency
for (const auto & topic : topic_map_) {
auto topic_name = topic.first;
RCLCPP_INFO(this->get_logger(), "Diagnostics config: Topic is %s", topic_name.c_str());
auto params_map = topic.second;

for(const auto & param : params_map) {
RCLCPP_INFO(this->get_logger(), " Param: %s = %s",
param.first.c_str(), param.second.value_to_string().c_str());
}

// Get the message type
std::string type;
try {
type = params_map.at("type").as_string();
} catch (const std::out_of_range & e) {
RCLCPP_ERROR(this->get_logger(), "No type provided for %s. This topic will not be monitored",
topic_name.c_str());
continue;
} catch(const rclcpp::ParameterTypeException & e) {
RCLCPP_ERROR(this->get_logger(),
"Type provided for %s was not a valid string. Value is %s. \
This topic will not be monitored",
topic_name.c_str(), params_map.at("type").value_to_string().c_str());
continue;
}

// Get the expected publishing rate
double rate = 0.0;
try {
rate = params_map.at("rate").as_double();
} catch (const std::out_of_range & e) {
RCLCPP_ERROR(this->get_logger(), "No rate provided for %s. This topic will not be monitored",
topic_name.c_str());
continue;
} catch(const rclcpp::ParameterTypeException & e) {
RCLCPP_ERROR(this->get_logger(),
"Rate provided for %s was not a valid double. Value is %s. \
This topic will not be monitored",
topic_name.c_str(), params_map.at("rate").value_to_string().c_str());
continue;
}

/*
* The section below does not use a generic subscription because the generic subscription was
* observed to have significantly hgiher CPU usage seemingly related to too short of callbacks
* and allocating/releasing the memory too quickly with Fast DDS. Standard subscriptions
* perform more reliably.
*/

// Create a subscription using the topic name and message type info from the yaml
if (type == "sensor_msgs/msg/CompressedImage") {
add_rate_diagnostic<sensor_msgs::msg::CompressedImage>(topic_name, rate);
} else if (type == "sensor_msgs/msg/Image") {
add_rate_diagnostic<sensor_msgs::msg::Image>(topic_name, rate);
} else if (type == "sensor_msgs/msg/Imu") {
add_rate_diagnostic<sensor_msgs::msg::Imu>(topic_name, rate);
} else if (type == "sensor_msgs/msg/LaserScan") {
add_rate_diagnostic<sensor_msgs::msg::LaserScan>(topic_name, rate);
} else if (type == "sensor_msgs/msg/MagneticField") {
add_rate_diagnostic<sensor_msgs::msg::MagneticField>(topic_name, rate);
} else if (type == "sensor_msgs/msg/NavSatFix") {
add_rate_diagnostic<sensor_msgs::msg::NavSatFix>(topic_name, rate);
} else if (type == "sensor_msgs/msg/PointCloud2") {
add_rate_diagnostic<sensor_msgs::msg::PointCloud2>(topic_name, rate);
} else {
RCLCPP_ERROR(this->get_logger(),
"Type \"%s\" of topic \"%s\" is not supported",
type.c_str(),
topic_name.c_str());
continue;
}

RCLCPP_INFO(this->get_logger(), "Created subscription for %s", topic_name.c_str());
}
}
}

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<clearpath::ClearpathDiagnosticUpdater>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
Original file line number Diff line number Diff line change
@@ -132,9 +132,28 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
],
)

# Diagnostics
# Diagnostics launch args
self.diag_updater_params = LaunchFile.LaunchArg(
'diagnostic_updater_params',
default_value=os.path.join(self.platform_params_path, 'diagnostic_updater.yaml'),
)
self.diag_aggregator_params = LaunchFile.LaunchArg(
'diagnostic_aggregator_params',
default_value=os.path.join(self.platform_params_path, 'diagnostic_aggregator.yaml'),
)

self.diagnostic_args = [
('namespace', self.namespace),
('updater_parameters', LaunchFile.Variable('diagnostic_updater_params')),
('aggregator_parameters', LaunchFile.Variable('diagnostic_aggregator_params')),
]

# Diagnostics launch
clearpath_diagnostics_package = Package('clearpath_diagnostics')
self.diagnostics_launch = LaunchFile('diagnostics', package=clearpath_diagnostics_package)
self.diagnostics_launch = LaunchFile(
'diagnostics',
package=clearpath_diagnostics_package,
args=self.diagnostic_args)

# Battery state
self.battery_state_estimator = LaunchFile.Node(
@@ -296,6 +315,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None:
# Components required for each platform
common_platform_components = [
self.wireless_watcher_node,
self.diag_updater_params,
self.diag_aggregator_params,
self.diagnostics_launch,
self.battery_state_control,
]
3 changes: 2 additions & 1 deletion clearpath_sensors/launch/axis_camera.launch.py
Original file line number Diff line number Diff line change
@@ -64,7 +64,8 @@ def generate_launch_description():
parameters=[parameters],
output='screen',
remappings=[
('image_raw/compressed', 'image/compressed'),
('image_raw/compressed', 'color/image/compressed'),
('camera_info', 'color/camera_info'),
('joint_states',
PathJoinSubstitution(['/', robot_namespace, 'platform', 'joint_states'])),
]

0 comments on commit 1394b88

Please sign in to comment.