diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 32df937..c221bc5 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -21,6 +21,12 @@ #include +#if HAS_ROS2_CONTROL_INTROSPECTION +namespace pal_statistics { +class StatisticsRegistry; +} // namespace pal_statistics +#endif + #if ROS2_VERSION_ABOVE_HUMBLE #include #else @@ -197,6 +203,15 @@ class CartesianController : public controller_interface::ControllerInterface { /** @brief Nullspace damping matrix for posture control */ Eigen::MatrixXd nullspace_damping; +#if HAS_ROS2_CONTROL_INTROSPECTION + /** + * @brief Registry of the debug values, created in on_configure() if enable_introspection is + * set. It is not the shared ros2_control registry: an activation or deactivation of the + * controller does not change it. + */ + std::shared_ptr debug_registry_; +#endif + /** @brief Current joint positions with dimension nv. */ Eigen::VectorXd q; /** @brief Current joint positions with dimension nq. diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index f1d8bd8..7793672 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -34,7 +34,7 @@ #include "crisp_controllers/utils/torque_rate_saturation.hpp" #if HAS_ROS2_CONTROL_INTROSPECTION -#include +#include #endif namespace crisp_controllers { @@ -234,6 +234,12 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & setStiffnessAndDamping(); } +#if HAS_ROS2_CONTROL_INTROSPECTION + if (debug_registry_) { + debug_registry_->publishAsync(); + } +#endif + log_debug_info(time); return controller_interface::return_type::OK; @@ -472,27 +478,37 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta nullspace_projection = Eigen::MatrixXd::Identity(model_.nv, model_.nv); #if HAS_ROS2_CONTROL_INTROSPECTION + debug_registry_.reset(); if (params_.enable_introspection) { - RCLCPP_INFO(get_node()->get_logger(), "Enabling ROS2 Control Introspection for debugging."); - this->enable_introspection(true); + // Own registry instead of the ros2_control registry: the controller manager enables and + // disables the values of that registry at each switch, in the realtime loop. + debug_registry_ = + std::make_shared(get_node(), "~/introspection_data"); + auto register_value = [this](const std::string & name, const double * value) { + debug_registry_->registerVariable(name, value); + }; for (int i = 0; i < tau_task.size(); ++i) { - REGISTER_ROS2_CONTROL_INTROSPECTION("tau_task_" + std::to_string(i), &tau_task[i]); - REGISTER_ROS2_CONTROL_INTROSPECTION("tau_desired_" + std::to_string(i), &tau_d[i]); + register_value("tau_task_" + std::to_string(i), &tau_task[i]); + register_value("tau_desired_" + std::to_string(i), &tau_d[i]); } for (int i = 0; i < error.size(); ++i) { - REGISTER_ROS2_CONTROL_INTROSPECTION("error_" + std::to_string(i), &error[i]); + register_value("error_" + std::to_string(i), &error[i]); } - REGISTER_ROS2_CONTROL_INTROSPECTION("target_position_x", &target_position_[0]); - REGISTER_ROS2_CONTROL_INTROSPECTION("target_position_y", &target_position_[1]); - REGISTER_ROS2_CONTROL_INTROSPECTION("target_position_z", &target_position_[2]); + register_value("target_position_x", &target_position_[0]); + register_value("target_position_y", &target_position_[1]); + register_value("target_position_z", &target_position_[2]); for (int i = 0; i < target_orientation_.coeffs().size(); ++i) { - REGISTER_ROS2_CONTROL_INTROSPECTION( + register_value( "target_orientation_" + std::to_string(i), &target_orientation_.coeffs()[i]); } for (int i = 0; i < q_target.size(); ++i) { - REGISTER_ROS2_CONTROL_INTROSPECTION("q_target_" + std::to_string(i), &q_target[i]); - REGISTER_ROS2_CONTROL_INTROSPECTION("q_ref_" + std::to_string(i), &q_ref[i]); + register_value("q_target_" + std::to_string(i), &q_target[i]); + register_value("q_ref_" + std::to_string(i), &q_ref[i]); } + debug_registry_->startPublishThread(); + RCLCPP_INFO( + get_node()->get_logger(), "Debug values are published on '%s/introspection_data/full'.", + get_node()->get_fully_qualified_name()); } else { RCLCPP_INFO(get_node()->get_logger(), "ROS2 Control Introspection is disabled."); } diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml index 01a17c8..c2f58e7 100644 --- a/src/cartesian_controller.yaml +++ b/src/cartesian_controller.yaml @@ -354,4 +354,4 @@ cartesian_controller: enable_introspection: type: bool default_value: false - description: "Whether or not to enable introspection for this controller. This will allow us to visualize the internal state of the controller in plotjuggler using the introspection topics." + description: "Whether or not to publish the internal state of this controller for debugging, e.g. in plotjuggler. The values are published on '/introspection_data/full'."