Skip to content

Commit 2121df0

Browse files
saikishorsaikishor
committed
Simplify registering and unregistering the statistics of HW and controllers
1 parent e62e458 commit 2121df0

File tree

2 files changed

+60
-115
lines changed

2 files changed

+60
-115
lines changed

‎controller_manager/src/controller_manager.cpp

Lines changed: 58 additions & 115 deletions
Original file line numberDiff line numberDiff line change
@@ -356,6 +356,23 @@ void get_controller_list_command_interfaces(
356356
}
357357
}
358358
}
359+
360+
void register_controller_manager_statistics(
361+
const std::string & name,
362+
const libstatistics_collector::moving_average_statistics::StatisticData * variable)
363+
{
364+
REGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name, variable);
365+
}
366+
367+
void unregister_controller_manager_statistics(const std::string & name)
368+
{
369+
UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/max");
370+
UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/min");
371+
UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/average");
372+
UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/standard_deviation");
373+
UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/sample_count");
374+
UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/current_value");
375+
}
359376
} // namespace
360377

361378
namespace controller_manager
@@ -722,87 +739,45 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
722739

723740
for (const auto & [component_name, component_info] : hw_components_info)
724741
{
742+
if (component_name.empty())
743+
{
744+
RCLCPP_WARN(
745+
get_logger(), "Component name is empty, skipping statistics registration for it.");
746+
continue;
747+
}
725748
RCLCPP_INFO(get_logger(), "Registering statistics for : %s", component_name.c_str());
749+
const std::string read_cycle_exec_time_prefix =
750+
component_name + ".stats/read_cycle/execution_time";
751+
const std::string read_cycle_periodicity_prefix =
752+
component_name + ".stats/read_cycle/periodicity";
753+
register_controller_manager_statistics(
754+
read_cycle_exec_time_prefix,
755+
&component_info.read_statistics->execution_time.get_statistics());
726756
REGISTER_ENTITY(
727-
hardware_interface::CM_STATISTICS_KEY,
728-
component_name + ".stats.read_cycle.execution_time.min",
729-
&component_info.read_statistics->execution_time.get_statistics().min);
730-
REGISTER_ENTITY(
731-
hardware_interface::CM_STATISTICS_KEY,
732-
component_name + ".stats.read_cycle.execution_time.max",
733-
&component_info.read_statistics->execution_time.get_statistics().max);
734-
REGISTER_ENTITY(
735-
hardware_interface::CM_STATISTICS_KEY,
736-
component_name + ".stats.read_cycle.execution_time.average",
737-
&component_info.read_statistics->execution_time.get_statistics().average);
738-
REGISTER_ENTITY(
739-
hardware_interface::CM_STATISTICS_KEY,
740-
component_name + ".stats.read_cycle.execution_time.stddev",
741-
&component_info.read_statistics->execution_time.get_statistics().standard_deviation);
742-
REGISTER_ENTITY(
743-
hardware_interface::CM_STATISTICS_KEY,
744-
component_name + ".stats.read_cycle.execution_time.current",
757+
hardware_interface::CM_STATISTICS_KEY, read_cycle_exec_time_prefix + "/current_value",
745758
&component_info.read_statistics->execution_time.get_current_data());
759+
register_controller_manager_statistics(
760+
read_cycle_periodicity_prefix, &component_info.read_statistics->periodicity.get_statistics());
746761
REGISTER_ENTITY(
747-
hardware_interface::CM_STATISTICS_KEY, component_name + ".stats.read_cycle.periodicity.min",
748-
&component_info.read_statistics->periodicity.get_statistics().min);
749-
REGISTER_ENTITY(
750-
hardware_interface::CM_STATISTICS_KEY, component_name + ".stats.read_cycle.periodicity.max",
751-
&component_info.read_statistics->periodicity.get_statistics().max);
752-
REGISTER_ENTITY(
753-
hardware_interface::CM_STATISTICS_KEY,
754-
component_name + ".stats.read_cycle.periodicity.average",
755-
&component_info.read_statistics->periodicity.get_statistics().average);
756-
REGISTER_ENTITY(
757-
hardware_interface::CM_STATISTICS_KEY,
758-
component_name + ".stats.read_cycle.periodicity.stddev",
759-
&component_info.read_statistics->periodicity.get_statistics().standard_deviation);
760-
REGISTER_ENTITY(
761-
hardware_interface::CM_STATISTICS_KEY,
762-
component_name + ".stats.read_cycle.periodicity.current",
762+
hardware_interface::CM_STATISTICS_KEY, read_cycle_periodicity_prefix + "/current_value",
763763
&component_info.read_statistics->periodicity.get_current_data());
764-
765764
if (component_info.write_statistics)
766765
{
766+
const std::string write_cycle_exec_time_prefix =
767+
component_name + ".stats/write_cycle/execution_time";
768+
const std::string write_cycle_periodicity_prefix =
769+
component_name + ".stats/write_cycle/periodicity";
770+
register_controller_manager_statistics(
771+
write_cycle_exec_time_prefix,
772+
&component_info.write_statistics->execution_time.get_statistics());
767773
REGISTER_ENTITY(
768-
hardware_interface::CM_STATISTICS_KEY,
769-
component_name + ".stats.write_cycle.execution_time.min",
770-
&component_info.write_statistics->execution_time.get_statistics().min);
771-
REGISTER_ENTITY(
772-
hardware_interface::CM_STATISTICS_KEY,
773-
component_name + ".stats.write_cycle.execution_time.max",
774-
&component_info.write_statistics->execution_time.get_statistics().max);
775-
REGISTER_ENTITY(
776-
hardware_interface::CM_STATISTICS_KEY,
777-
component_name + ".stats.write_cycle.execution_time.average",
778-
&component_info.write_statistics->execution_time.get_statistics().average);
779-
REGISTER_ENTITY(
780-
hardware_interface::CM_STATISTICS_KEY,
781-
component_name + ".stats.write_cycle.execution_time.stddev",
782-
&component_info.write_statistics->execution_time.get_statistics().standard_deviation);
783-
REGISTER_ENTITY(
784-
hardware_interface::CM_STATISTICS_KEY,
785-
component_name + ".stats.write_cycle.execution_time.current",
774+
hardware_interface::CM_STATISTICS_KEY, write_cycle_exec_time_prefix + "/current_value",
786775
&component_info.write_statistics->execution_time.get_current_data());
776+
register_controller_manager_statistics(
777+
write_cycle_periodicity_prefix,
778+
&component_info.write_statistics->periodicity.get_statistics());
787779
REGISTER_ENTITY(
788-
hardware_interface::CM_STATISTICS_KEY,
789-
component_name + ".stats.write_cycle.periodicity.min",
790-
&component_info.write_statistics->periodicity.get_statistics().min);
791-
REGISTER_ENTITY(
792-
hardware_interface::CM_STATISTICS_KEY,
793-
component_name + ".stats.write_cycle.periodicity.max",
794-
&component_info.write_statistics->periodicity.get_statistics().max);
795-
REGISTER_ENTITY(
796-
hardware_interface::CM_STATISTICS_KEY,
797-
component_name + ".stats.write_cycle.periodicity.average",
798-
&component_info.write_statistics->periodicity.get_statistics().average);
799-
REGISTER_ENTITY(
800-
hardware_interface::CM_STATISTICS_KEY,
801-
component_name + ".stats.write_cycle.periodicity.stddev",
802-
&component_info.write_statistics->periodicity.get_statistics().standard_deviation);
803-
REGISTER_ENTITY(
804-
hardware_interface::CM_STATISTICS_KEY,
805-
component_name + ".stats.write_cycle.periodicity.current",
780+
hardware_interface::CM_STATISTICS_KEY, write_cycle_periodicity_prefix + "/current_value",
806781
&component_info.write_statistics->periodicity.get_current_data());
807782
}
808783
}
@@ -949,35 +924,19 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
949924
std::make_shared<rclcpp::Time>(0, 0, this->get_trigger_clock()->get_clock_type());
950925
controller_spec.execution_time_statistics = std::make_shared<MovingAverageStatistics>();
951926
controller_spec.periodicity_statistics = std::make_shared<MovingAverageStatistics>();
927+
const std::string controller_exec_time_prefix = controller_name + ".stats/execution_time";
928+
const std::string controller_periodicity_prefix = controller_name + ".stats/periodicity";
929+
register_controller_manager_statistics(
930+
controller_exec_time_prefix,
931+
&controller_spec.execution_time_statistics->get_statistics_const_ptr());
952932
REGISTER_ENTITY(
953-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.execution_time.min",
954-
&controller_spec.execution_time_statistics->get_statistics_const_ptr().min);
955-
REGISTER_ENTITY(
956-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.execution_time.max",
957-
&controller_spec.execution_time_statistics->get_statistics_const_ptr().max);
958-
REGISTER_ENTITY(
959-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.execution_time.average",
960-
&controller_spec.execution_time_statistics->get_statistics_const_ptr().average);
961-
REGISTER_ENTITY(
962-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.execution_time.stddev",
963-
&controller_spec.execution_time_statistics->get_statistics_const_ptr().standard_deviation);
964-
REGISTER_ENTITY(
965-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.execution_time.current",
933+
hardware_interface::CM_STATISTICS_KEY, controller_exec_time_prefix + "/current_value",
966934
&controller_spec.execution_time_statistics->get_current_measurement_const_ptr());
935+
register_controller_manager_statistics(
936+
controller_periodicity_prefix,
937+
&controller_spec.periodicity_statistics->get_statistics_const_ptr());
967938
REGISTER_ENTITY(
968-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.periodicity.min",
969-
&controller_spec.periodicity_statistics->get_statistics_const_ptr().min);
970-
REGISTER_ENTITY(
971-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.periodicity.max",
972-
&controller_spec.periodicity_statistics->get_statistics_const_ptr().max);
973-
REGISTER_ENTITY(
974-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.periodicity.average",
975-
&controller_spec.periodicity_statistics->get_statistics_const_ptr().average);
976-
REGISTER_ENTITY(
977-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.periodicity.stddev",
978-
&controller_spec.periodicity_statistics->get_statistics_const_ptr().standard_deviation);
979-
REGISTER_ENTITY(
980-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.periodicity.current",
939+
hardware_interface::CM_STATISTICS_KEY, controller_periodicity_prefix + "/current_value",
981940
&controller_spec.periodicity_statistics->get_current_measurement_const_ptr());
982941

983942
// We have to fetch the parameters_file at the time of loading the controller, because this way we
@@ -1118,24 +1077,8 @@ controller_interface::return_type ControllerManager::unload_controller(
11181077
get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str());
11191078
shutdown_controller(controller);
11201079
}
1121-
UNREGISTER_ENTITY(
1122-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.execution_time.min");
1123-
UNREGISTER_ENTITY(
1124-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.execution_time.max");
1125-
UNREGISTER_ENTITY(
1126-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.execution_time.average");
1127-
UNREGISTER_ENTITY(
1128-
hardware_interface::CM_STATISTICS_KEY,
1129-
controller_name + ".stats.execution_time.current_measurement");
1130-
UNREGISTER_ENTITY(
1131-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.periodicity.min");
1132-
UNREGISTER_ENTITY(
1133-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.periodicity.max");
1134-
UNREGISTER_ENTITY(
1135-
hardware_interface::CM_STATISTICS_KEY, controller_name + ".stats.periodicity.average");
1136-
UNREGISTER_ENTITY(
1137-
hardware_interface::CM_STATISTICS_KEY,
1138-
controller_name + ".stats.periodicity.current_measurement");
1080+
unregister_controller_manager_statistics(controller_name + ".stats/execution_time");
1081+
unregister_controller_manager_statistics(controller_name + ".stats/periodicity");
11391082
executor_->remove_node(controller.c->get_node()->get_node_base_interface());
11401083
to.erase(found_it);
11411084

‎hardware_interface/include/hardware_interface/introspection.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@
1717
#ifndef HARDWARE_INTERFACE__INTROSPECTION_HPP_
1818
#define HARDWARE_INTERFACE__INTROSPECTION_HPP_
1919

20+
#include <string>
21+
2022
#include "hardware_interface/types/statistics_types.hpp"
2123
#include "pal_statistics/pal_statistics_macros.hpp"
2224
#include "pal_statistics/pal_statistics_utils.hpp"

0 commit comments

Comments
 (0)