Integrate pal_statistics for introspecting the ros2_control_demos

ros-controls · Nov 28, 2024 · ce48595 · ce48595
1 parent 7b7f70c
commit ce48595
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 0 deletions.
diff --git a/example_12/controllers/src/passthrough_controller.cpp b/example_12/controllers/src/passthrough_controller.cpp
@@ -85,6 +85,11 @@ controller_interface::CallbackReturn PassthroughController::on_configure(
   reference_interfaces_.resize(
     reference_interface_names_.size(), std::numeric_limits<double>::quiet_NaN());
 
+  for (size_t i = 0; i < reference_interface_names_.size(); i++)
+  {
+    REGISTER_DEFAULT_INTROSPECTION(reference_interface_names_[i], &reference_interfaces_[i]);
+  }
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 

diff --git a/example_12/hardware/rrbot.cpp b/example_12/hardware/rrbot.cpp
@@ -83,6 +83,15 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init(
     }
   }
 
+  REGISTER_DEFAULT_INTROSPECTION("hw_start_sec", &hw_start_sec_);
+  REGISTER_DEFAULT_INTROSPECTION("hw_stop_sec", &hw_stop_sec_);
+  REGISTER_DEFAULT_INTROSPECTION("hw_slowdown", &hw_slowdown_);
+  for (size_t i = 0; i < info_.joints.size(); ++i)
+  {
+    REGISTER_DEFAULT_INTROSPECTION(info_.joints[i].name + ".hw_state", &hw_states_[i]);
+    REGISTER_DEFAULT_INTROSPECTION(info_.joints[i].name + ".hw_command", &hw_commands_[i]);
+  }
+
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 

diff --git a/example_4/hardware/rrbot_system_with_sensor.cpp b/example_4/hardware/rrbot_system_with_sensor.cpp
@@ -90,6 +90,22 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init(
     }
   }
 
+  REGISTER_DEFAULT_INTROSPECTION("hw_start_sec", &hw_start_sec_);
+  REGISTER_DEFAULT_INTROSPECTION("hw_stop_sec", &hw_stop_sec_);
+  REGISTER_DEFAULT_INTROSPECTION("hw_slowdown", &hw_slowdown_);
+  REGISTER_DEFAULT_INTROSPECTION("hw_sensor_change", &hw_sensor_change_);
+  for (size_t i = 0; i < info_.joints.size(); ++i)
+  {
+    REGISTER_DEFAULT_INTROSPECTION(info_.joints[i].name + ".hw_state", &hw_joint_states_[i]);
+    REGISTER_DEFAULT_INTROSPECTION(info_.joints[i].name + ".hw_command", &hw_joint_commands_[i]);
+  }
+  for (size_t i = 0; i < info_.sensors[0].state_interfaces.size(); ++i)
+  {
+    REGISTER_DEFAULT_INTROSPECTION(
+      info_.sensors[0].name + "." + info_.sensors[0].state_interfaces[i].name,
+      &hw_sensor_states_[i]);
+  }
+
   return hardware_interface::CallbackReturn::SUCCESS;
 }