From 2c609d0579e633f157cb0e8a79fbf6904bc6e5e0 Mon Sep 17 00:00:00 2001 From: David Alexander Date: Sun, 7 Jun 2026 19:54:00 -0400 Subject: [PATCH 1/3] feat: state_broadcaster implementation --- CMakeLists.txt | 21 +- crisp_controllers.xml | 12 +- .../cartesian_controller.hpp | 14 + .../crisp_controllers/pose_broadcaster.hpp | 77 -- .../crisp_controllers/state_broadcaster.hpp | 342 +++++++++ .../crisp_controllers/twist_broadcaster.hpp | 78 -- src/cartesian_controller.cpp | 60 +- src/cartesian_controller.yaml | 12 +- src/pose_broadcaster.cpp | 224 ------ src/pose_broadcaster.yaml | 15 - src/state_broadcaster.cpp | 724 ++++++++++++++++++ src/state_broadcaster.yaml | 125 +++ src/twist_broadcaster.cpp | 231 ------ src/twist_broadcaster.yaml | 15 - 14 files changed, 1288 insertions(+), 662 deletions(-) delete mode 100644 include/crisp_controllers/pose_broadcaster.hpp create mode 100644 include/crisp_controllers/state_broadcaster.hpp delete mode 100644 include/crisp_controllers/twist_broadcaster.hpp delete mode 100644 src/pose_broadcaster.cpp delete mode 100644 src/pose_broadcaster.yaml create mode 100644 src/state_broadcaster.cpp create mode 100644 src/state_broadcaster.yaml delete mode 100644 src/twist_broadcaster.cpp delete mode 100644 src/twist_broadcaster.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 94fa9f3..56ddbb4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,6 +61,8 @@ find_package(Eigen3 REQUIRED) find_package(pinocchio REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) generate_parameter_library( @@ -69,8 +71,8 @@ generate_parameter_library( ) generate_parameter_library( - pose_broadcaster_parameters - src/pose_broadcaster.yaml + state_broadcaster_parameters + src/state_broadcaster.yaml ) generate_parameter_library( @@ -78,11 +80,6 @@ generate_parameter_library( src/torque_feedback_controller.yaml ) -generate_parameter_library( - twist_broadcaster_parameters - src/twist_broadcaster.yaml -) - generate_parameter_library( cartesian_admittance_controller_parameters src/cartesian_admittance_controller.yaml @@ -93,9 +90,8 @@ add_library( SHARED src/cartesian_controller.cpp src/cartesian_admittance_controller.cpp - src/pose_broadcaster.cpp + src/state_broadcaster.cpp src/torque_feedback_controller.cpp - src/twist_broadcaster.cpp ) target_link_libraries(${PROJECT_NAME} @@ -121,9 +117,8 @@ target_link_libraries(${PROJECT_NAME} PRIVATE cartesian_controller_parameters cartesian_admittance_controller_parameters - pose_broadcaster_parameters + state_broadcaster_parameters torque_feedback_controller_parameters - twist_broadcaster_parameters ) @@ -138,6 +133,8 @@ ament_target_dependencies(${PROJECT_NAME} generate_parameter_library realtime_tools std_msgs + geometry_msgs + sensor_msgs ) pluginlib_export_plugin_description_file( @@ -166,6 +163,8 @@ ament_export_dependencies( pinocchio realtime_tools std_msgs + geometry_msgs + sensor_msgs generate_parameter_library ) diff --git a/crisp_controllers.xml b/crisp_controllers.xml index 3784f2f..50aad2c 100644 --- a/crisp_controllers.xml +++ b/crisp_controllers.xml @@ -11,10 +11,10 @@ Cartesian admittance controller with impedance outer loop for compliant force-based interaction. - + - Simple broadcaster for the pose. + Broadcaster for joint, Cartesian pose, twist, and wrench state outputs. - - - Simple broadcaster for the twist velocity. - - diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 32df937..315f3f5 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include @@ -106,6 +107,14 @@ class CartesianController : public controller_interface::ControllerInterface { /** @brief Subscription for variable stiffness messages */ rclcpp::Subscription::SharedPtr stiffness_sub_; + /** @brief Publisher for commanded joint effort values */ + rclcpp::Publisher::SharedPtr effort_publisher_; + /** @brief Realtime publisher for commanded joint effort values */ + std::shared_ptr> + realtime_effort_publisher_; + /** @brief Pre-sized commanded effort message for realtime publication */ + std_msgs::msg::Float64MultiArray effort_msg_; + /** @brief Flag to indicate if multiple publishers detected */ bool multiple_publishers_detected_; @@ -274,6 +283,11 @@ class CartesianController : public controller_interface::ControllerInterface { /** @brief Final desired torque command */ Eigen::VectorXd tau_d; + /** @brief Time accumulated since the last effort publication */ + rclcpp::Duration publish_elapsed_{0, 0}; + /** @brief Effort publication interval */ + rclcpp::Duration publish_interval_{0, 0}; + /** @brief Inverse of the manipulator joint mass projected in Cartesian space (6x6) */ Eigen::Matrix Mx_inv = Eigen::Matrix::Zero(); /** @brief the manipulator joint mass projected in Cartesian space (6x6) */ diff --git a/include/crisp_controllers/pose_broadcaster.hpp b/include/crisp_controllers/pose_broadcaster.hpp deleted file mode 100644 index 46fb317..0000000 --- a/include/crisp_controllers/pose_broadcaster.hpp +++ /dev/null @@ -1,77 +0,0 @@ -#pragma once - -#include -#include -#include - -#include // NOLINT(build/include_order) - -#include -#include - -#if ROS2_VERSION_ABOVE_HUMBLE -#include -#else -#include -#endif - -#include -#include -#include - -#include -#include -#include - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -namespace crisp_controllers { - -class PoseBroadcaster : public controller_interface::ControllerInterface { -public: - [[nodiscard]] controller_interface::InterfaceConfiguration - command_interface_configuration() const override; - [[nodiscard]] controller_interface::InterfaceConfiguration - state_interface_configuration() const override; - controller_interface::return_type - update(const rclcpp::Time & time, const rclcpp::Duration & period) override; - CallbackReturn on_init() override; - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - -private: - std::shared_ptr params_listener_; - pose_broadcaster::Params params_; - - rclcpp::Publisher::SharedPtr pose_publisher_; - std::shared_ptr> - realtime_pose_publisher_; - - std::string end_effector_frame_; - int end_effector_frame_id; - - pinocchio::Model model_; - pinocchio::Data data_; - - /** @brief Allowed type of joints **/ - const std::unordered_set> allowed_joint_types = { - "JointModelRX", - "JointModelRY", - "JointModelRZ", - "JointModelRevoluteUnaligned", - "JointModelRUBX", - "JointModelRUBY", - "JointModelRUBZ", - }; - /** @brief Continous joint types that should be considered separetly. **/ - const std::unordered_set> continous_joint_types = { - "JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; - - Eigen::VectorXd q; - - rclcpp::Duration publish_elapsed_{0, 0}; - rclcpp::Duration publish_interval_{0, 0}; -}; - -} // namespace crisp_controllers diff --git a/include/crisp_controllers/state_broadcaster.hpp b/include/crisp_controllers/state_broadcaster.hpp new file mode 100644 index 0000000..5fc67da --- /dev/null +++ b/include/crisp_controllers/state_broadcaster.hpp @@ -0,0 +1,342 @@ +#pragma once + +/** + * @file state_broadcaster.hpp + * @brief State-only ROS 2 controller that publishes robot pose, twist, wrench, residual effort, + * and joint state from hardware state interfaces. + */ + +#include +#include +#include +#include +#include + +#include // NOLINT(build/include_order) + +#include +#include + +#if ROS2_VERSION_ABOVE_HUMBLE +#include +#else +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace crisp_controllers { + +/** + * @brief ROS 2 controller_interface plugin for broadcasting robot state estimates. + * + * StateBroadcaster consumes state interfaces only and publishes end-effector pose, optional twist, + * optional raw wrench estimates, optional residual effort after subtracting enabled model terms, + * optional wrench estimates projected from residual effort, and joint state messages. Pinocchio is + * used for kinematics, frame Jacobians, model terms, and wrench estimation. Outputs that require + * velocity or effort state interfaces are enabled only when the complete corresponding interface + * group is available. + */ +class StateBroadcaster : public controller_interface::ControllerInterface { +public: + /** + * @brief Declare that this broadcaster does not claim command interfaces. + * @return Interface configuration with type NONE. + */ + [[nodiscard]] controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + /** + * @brief Declare that this broadcaster reads available state interfaces. + * @return Interface configuration with type ALL. + */ + [[nodiscard]] controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + /** + * @brief Read state interfaces, update Pinocchio state, and publish configured outputs. + * @param time Current controller manager time used for message stamps. + * @param period Time elapsed since the previous update. + * @return OK when the update completes. + */ + controller_interface::return_type + update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + /** + * @brief Initialize generated parameter handling. + * @return Lifecycle callback result. + */ + CallbackReturn on_init() override; + + /** + * @brief Validate parameters, build the reduced Pinocchio model, and create publishers. + * @param previous_state Previous lifecycle state. + * @return Lifecycle callback result. + */ + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Cache state interface indices and initialize publication state. + * @param previous_state Previous lifecycle state. + * @return Lifecycle callback result. + */ + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Deactivate the broadcaster. + * @param previous_state Previous lifecycle state. + * @return Lifecycle callback result. + */ + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + +private: + /** @brief Sentinel index used when a requested state interface is unavailable. */ + static constexpr size_t kMissingInterface = static_cast(-1); + + /** @brief Accumulated elapsed time and configured interval for a periodic output. */ + struct PublishTimer { + /** @brief Time accumulated since this output last published. */ + rclcpp::Duration elapsed{0, 0}; + /** @brief Desired interval between publications; zero means publish every update. */ + rclcpp::Duration interval{0, 0}; + }; + + /** @brief Generated parameter listener for state broadcaster parameters. */ + std::shared_ptr params_listener_; + /** @brief Cached parameter values used by the broadcaster. */ + state_broadcaster::Params params_; + + /** @brief Publisher for end-effector pose messages. */ + rclcpp::Publisher::SharedPtr pose_publisher_; + /** @brief Realtime publisher for end-effector pose messages. */ + std::shared_ptr> + realtime_pose_publisher_; + /** @brief Preallocated end-effector pose message. */ + geometry_msgs::msg::PoseStamped pose_msg_; + + /** @brief Publisher for end-effector twist messages. */ + rclcpp::Publisher::SharedPtr twist_publisher_; + /** @brief Realtime publisher for end-effector twist messages. */ + std::shared_ptr> + realtime_twist_publisher_; + /** @brief Preallocated end-effector twist message. */ + geometry_msgs::msg::TwistStamped twist_msg_; + + /** @brief Publisher for wrench estimated directly from measured joint effort. */ + rclcpp::Publisher::SharedPtr raw_wrench_publisher_; + /** @brief Realtime publisher for wrench estimated directly from measured joint effort. */ + std::shared_ptr> + realtime_raw_wrench_publisher_; + /** @brief Preallocated raw wrench message. */ + geometry_msgs::msg::WrenchStamped raw_wrench_msg_; + + /** @brief Publisher for wrench estimated from residual effort. */ + rclcpp::Publisher::SharedPtr external_wrench_publisher_; + /** @brief Realtime publisher for wrench estimated from residual effort. */ + std::shared_ptr> + realtime_external_wrench_publisher_; + /** @brief Preallocated external wrench message. */ + geometry_msgs::msg::WrenchStamped external_wrench_msg_; + + /** @brief Publisher for residual joint effort after subtracting enabled model terms. */ + rclcpp::Publisher::SharedPtr external_effort_publisher_; + /** @brief Realtime publisher for residual joint effort after subtracting enabled model terms. */ + std::shared_ptr> + realtime_external_effort_publisher_; + /** @brief Preallocated external effort message. */ + std_msgs::msg::Float64MultiArray external_effort_msg_; + + /** @brief Publisher for joint state messages assembled from available state interfaces. */ + rclcpp::Publisher::SharedPtr joint_state_publisher_; + /** @brief Realtime publisher for joint state messages. */ + std::shared_ptr> + realtime_joint_state_publisher_; + /** @brief Preallocated joint state message. */ + sensor_msgs::msg::JointState joint_state_msg_; + + /** @brief Pinocchio frame index for the configured end-effector frame. */ + pinocchio::FrameIndex end_effector_frame_id_; + /** @brief Reference frame used when computing and publishing end-effector twist. */ + pinocchio::ReferenceFrame twist_reference_frame_{pinocchio::ReferenceFrame::LOCAL}; + /** @brief Reference frame used when estimating raw wrench. */ + pinocchio::ReferenceFrame raw_wrench_reference_frame_{ + pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED}; + /** @brief Reference frame used when estimating external wrench. */ + pinocchio::ReferenceFrame external_wrench_reference_frame_{ + pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED}; + + /** @brief Reduced Pinocchio model containing the configured joints. */ + pinocchio::Model model_; + /** @brief Pinocchio data workspace for kinematics, dynamics, and Jacobian computations. */ + pinocchio::Data data_; + + /** @brief Pinocchio joint model short names accepted by this broadcaster. */ + const std::unordered_set> allowed_joint_types_ = { + "JointModelRX", + "JointModelRY", + "JointModelRZ", + "JointModelRevoluteUnaligned", + "JointModelRUBX", + "JointModelRUBY", + "JointModelRUBZ", + }; + /** @brief Pinocchio continuous joint model short names represented with cosine/sine positions. */ + const std::unordered_set> continuous_joint_types_ = { + "JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; + + /** @brief Pinocchio q-vector indices for configured joints. */ + std::vector joint_q_indices_; + /** @brief Pinocchio velocity and effort vector indices for configured joints. */ + std::vector joint_v_indices_; + /** @brief Flags indicating which configured joints use cosine/sine position representation. */ + std::vector joint_is_continuous_; + /** @brief Controller state interface indices for joint position values. */ + std::vector position_interface_indices_; + /** @brief Controller state interface indices for joint velocity values. */ + std::vector velocity_interface_indices_; + /** @brief Controller state interface indices for joint effort values. */ + std::vector effort_interface_indices_; + /** @brief True when every configured joint has a velocity state interface. */ + bool has_velocity_interfaces_{false}; + /** @brief True when every configured joint has an effort state interface. */ + bool has_effort_interfaces_{false}; + + /** @brief Current configured joint positions in parameter order. */ + Eigen::VectorXd q_; + /** @brief Current Pinocchio configuration vector. */ + Eigen::VectorXd q_pin_; + /** @brief Current Pinocchio velocity vector. */ + Eigen::VectorXd dq_; + /** @brief Previous Pinocchio velocity vector used for acceleration estimation. */ + Eigen::VectorXd dq_previous_; + /** @brief Estimated joint acceleration before filtering. */ + Eigen::VectorXd ddq_estimated_; + /** @brief Filtered joint acceleration used for inertial compensation. */ + Eigen::VectorXd ddq_filtered_; + /** @brief Measured joint effort vector read from state interfaces. */ + Eigen::VectorXd tau_measured_; + /** @brief Coriolis effort compensation vector. */ + Eigen::VectorXd tau_coriolis_; + /** @brief Gravity effort compensation vector. */ + Eigen::VectorXd tau_gravity_; + /** @brief Inertial effort compensation vector. */ + Eigen::VectorXd tau_inertia_; + /** @brief Residual effort after subtracting enabled model compensation terms. */ + Eigen::VectorXd tau_residual_; + /** @brief End-effector frame Jacobian workspace. */ + pinocchio::Data::Matrix6x J_; + /** @brief Regularized wrench least-squares system matrix. */ + Eigen::Matrix wrench_system_; + /** @brief Wrench least-squares right-hand side vector. */ + Eigen::Matrix wrench_rhs_; + /** @brief Wrench estimated from measured joint effort. */ + Eigen::Matrix raw_wrench_; + /** @brief Wrench estimated from residual joint effort. */ + Eigen::Matrix external_wrench_; + /** @brief True after the previous velocity vector has been initialized. */ + bool has_previous_velocity_{false}; + + /** @brief Publish timer for pose output. */ + PublishTimer pose_timer_; + /** @brief Publish timer for twist output. */ + PublishTimer twist_timer_; + /** @brief Publish timer for raw wrench output. */ + PublishTimer raw_wrench_timer_; + /** @brief Publish timer for external wrench output. */ + PublishTimer external_wrench_timer_; + /** @brief Publish timer for external effort output. */ + PublishTimer external_effort_timer_; + /** @brief Publish timer for joint state output. */ + PublishTimer joint_state_timer_; + + /** + * @brief Parse an output reference-frame parameter into Pinocchio and ROS frame settings. + * @param output_name Output name used in error messages. + * @param reference_frame Parameter value to parse. + * @param parsed_reference_frame Parsed Pinocchio reference frame. + * @param published_frame Frame ID associated with the parsed reference frame. + * @return True when the reference frame value is supported. + */ + bool configure_reference_frame( + const std::string & output_name, const std::string & reference_frame, + pinocchio::ReferenceFrame & parsed_reference_frame, std::string & published_frame) const; + + /** + * @brief Convert an output publish frequency parameter into a publish interval. + * @param output_name Output name used in error messages. + * @param publish_frequency Publish frequency in hertz; zero publishes every update. + * @param timer Timer receiving the configured interval. + * @return True when the frequency is valid. + */ + bool configure_publish_interval( + const std::string & output_name, double publish_frequency, PublishTimer & timer) const; + + /** + * @brief Validate common parameters required before model and publisher setup. + * @return True when required joints, frames, and topics are configured. + */ + bool validate_common_parameters() const; + + /** + * @brief Build the reduced Pinocchio model and allocate model-sized work buffers. + * @return True when the robot description and configured frames/joints are valid. + */ + bool build_model(); + + /** + * @brief Cache Pinocchio position and velocity indices for configured joints. + * @return True when indices are cached. + */ + bool cache_joint_model_indices(); + + /** + * @brief Cache controller state interface indices and detect optional interface groups. + * @return True when all required position state interfaces are available. + */ + bool cache_state_interface_indices(); + + /** @brief Read joint position state interfaces into the configured joint vector. */ + void read_position_interfaces(); + + /** @brief Read joint velocity state interfaces into the Pinocchio velocity vector. */ + void read_velocity_interfaces(); + + /** @brief Read joint effort state interfaces into the measured effort vector. */ + void read_effort_interfaces(); + + /** @brief Convert configured joint positions into Pinocchio configuration representation. */ + void update_pinocchio_positions(); + + /** + * @brief Estimate a spatial wrench from joint effort using the end-effector Jacobian. + * @param effort Joint effort vector in Pinocchio velocity order. + * @param reference_frame Pinocchio reference frame for the frame Jacobian. + * @param wrench Output spatial wrench vector. + */ + void compute_wrench(const Eigen::VectorXd & effort, pinocchio::ReferenceFrame reference_frame, + Eigen::Matrix & wrench); + + /** @brief Copy residual efforts into the preallocated external effort message. */ + void update_external_effort_message(); + + /** + * @brief Advance a publish timer and report whether its output should publish this update. + * @param period Time elapsed since the previous update. + * @param timer Publish timer to update. + * @return True when the output should publish. + */ + bool should_publish(const rclcpp::Duration & period, PublishTimer & timer); +}; + +} // namespace crisp_controllers diff --git a/include/crisp_controllers/twist_broadcaster.hpp b/include/crisp_controllers/twist_broadcaster.hpp deleted file mode 100644 index f4e7694..0000000 --- a/include/crisp_controllers/twist_broadcaster.hpp +++ /dev/null @@ -1,78 +0,0 @@ -#pragma once - -#include -#include -#include - -#include // NOLINT(build/include_order) - -#include -#include - -#if ROS2_VERSION_ABOVE_HUMBLE -#include -#else -#include -#endif - -#include -#include -#include - -#include -#include -#include - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -namespace crisp_controllers { - -class TwistBroadcaster : public controller_interface::ControllerInterface { -public: - [[nodiscard]] controller_interface::InterfaceConfiguration - command_interface_configuration() const override; - [[nodiscard]] controller_interface::InterfaceConfiguration - state_interface_configuration() const override; - controller_interface::return_type - update(const rclcpp::Time & time, const rclcpp::Duration & period) override; - CallbackReturn on_init() override; - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - -private: - std::shared_ptr params_listener_; - twist_broadcaster::Params params_; - - rclcpp::Publisher::SharedPtr twist_publisher_; - std::shared_ptr> - realtime_twist_publisher_; - - std::string end_effector_frame_; - int end_effector_frame_id; - - pinocchio::Model model_; - pinocchio::Data data_; - - /** @brief Allowed type of joints **/ - const std::unordered_set> allowed_joint_types = { - "JointModelRX", - "JointModelRY", - "JointModelRZ", - "JointModelRevoluteUnaligned", - "JointModelRUBX", - "JointModelRUBY", - "JointModelRUBZ", - }; - /** @brief Continous joint types that should be considered separetly. **/ - const std::unordered_set> continous_joint_types = { - "JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; - - Eigen::VectorXd q; - Eigen::VectorXd q_dot; - - rclcpp::Duration publish_elapsed_{0, 0}; - rclcpp::Duration publish_interval_{0, 0}; -}; - -} // namespace crisp_controllers diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 18a4668..61a78eb 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -64,7 +64,7 @@ CartesianController::state_interface_configuration() const { } controller_interface::return_type -CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { +CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & period) { // Update current state information with EMA filtered values updateCurrentState(); @@ -226,6 +226,30 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & } } + publish_elapsed_ = publish_elapsed_ + period; + const bool should_publish = + realtime_effort_publisher_ && + ((publish_elapsed_ >= publish_interval_) || (publish_interval_.nanoseconds() == 0)); + if (should_publish) { +#if REALTIME_TOOLS_NEW_API + for (size_t i = 0; i < effort_msg_.data.size(); ++i) { + effort_msg_.data[i] = params_.stop_commands ? 0.0 : tau_d[i]; + } + if (realtime_effort_publisher_->try_publish(effort_msg_)) { +#else + if (realtime_effort_publisher_->trylock()) { + auto & effort_data = realtime_effort_publisher_->msg_.data; + for (size_t i = 0; i < effort_data.size(); ++i) { + effort_data[i] = params_.stop_commands ? 0.0 : tau_d[i]; + } + realtime_effort_publisher_->unlockAndPublish(); +#endif + publish_elapsed_ = publish_elapsed_ - publish_interval_; + // clamp to publish only 1 time even if missed multiple intervals + publish_elapsed_ = std::min(publish_elapsed_, publish_interval_); + } + } + tau_previous = tau_d; params_listener_->refresh_dynamic_parameters(); @@ -446,6 +470,38 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta tau_wrench = Eigen::VectorXd::Zero(model_.nv); tau_d = Eigen::VectorXd::Zero(model_.nv); + effort_publisher_.reset(); + realtime_effort_publisher_.reset(); + publish_elapsed_ = rclcpp::Duration(0, 0); + publish_interval_ = rclcpp::Duration(0, 0); + if (params_.effort_publisher.publish_frequency > 0.0) { + if (params_.effort_publisher.topic.empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because effort_publisher.topic is empty."); + return CallbackReturn::ERROR; + } + effort_publisher_ = get_node()->create_publisher( + params_.effort_publisher.topic, rclcpp::SystemDefaultsQoS()); + realtime_effort_publisher_ = + std::make_shared>( + effort_publisher_); +#if !REALTIME_TOOLS_NEW_API + realtime_effort_publisher_->msg_.data.resize(params_.joints.size(), 0.0); +#else + effort_msg_.data.resize(params_.joints.size(), 0.0); +#endif + publish_interval_ = + rclcpp::Duration::from_seconds(1.0 / params_.effort_publisher.publish_frequency); + RCLCPP_INFO( + get_node()->get_logger(), + "Commanded effort topic: %s at %.3f Hz", + params_.effort_publisher.topic.c_str(), + params_.effort_publisher.publish_frequency); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Commanded effort publisher is disabled."); + } + // Initialize target state vectors target_position_ = Eigen::Vector3d::Zero(); target_orientation_ = Eigen::Quaterniond::Identity(); @@ -602,6 +658,8 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat desired_position_ = target_position_; desired_orientation_ = target_orientation_; + publish_elapsed_ = rclcpp::Duration(0, 0); + RCLCPP_INFO(get_node()->get_logger(), "Controller activated."); return CallbackReturn::SUCCESS; } diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml index a19f68d..24fdb00 100644 --- a/src/cartesian_controller.yaml +++ b/src/cartesian_controller.yaml @@ -181,7 +181,7 @@ cartesian_controller: use_coriolis_compensation: type: bool - default_value: false + default_value: true description: "Use coriolis compensation" use_gravity_compensation: @@ -299,6 +299,16 @@ cartesian_controller: default_value: False description: "If set to true, we will stop sending commands to the robot. This is mainly for debugging useful." + effort_publisher: + topic: + type: string + default_value: "commanded_effort" + description: "Topic name for publishing commanded joint efforts as std_msgs/Float64MultiArray. Values are in joints order." + publish_frequency: + type: double + default_value: 1000.0 + description: "Frequency in Hz for publishing commanded joint efforts. Values <= 0.0 disable publishing." + friction: fp1: type: double_array diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp deleted file mode 100644 index a88d460..0000000 --- a/src/pose_broadcaster.cpp +++ /dev/null @@ -1,224 +0,0 @@ -#include -#include -#include -#include - -#include // NOLINT(build/include_order) -#include -#include "crisp_controllers/utils/ros2_version.hpp" - -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; - -namespace crisp_controllers { - -controller_interface::InterfaceConfiguration -PoseBroadcaster::command_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::NONE; - return config; -} - -controller_interface::InterfaceConfiguration -PoseBroadcaster::state_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto & joint_name : params_.joints) { - config.names.push_back(joint_name + "/position"); - } - return config; -} - -controller_interface::return_type -PoseBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & period) { - size_t num_joints = params_.joints.size(); - Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq); - - for (size_t i = 0; i < num_joints; i++) { - auto joint_name = params_.joints[i]; - auto joint_id = model_.getJointId(joint_name); - auto joint = model_.joints[joint_id]; - -#if ROS2_VERSION_ABOVE_HUMBLE - q[i] = state_interfaces_[i].get_optional().value_or(q[i]); -#else - q[i] = state_interfaces_[i].get_value(); -#endif - if (continous_joint_types.count( - joint.shortname())) { // Then we are handling a continous joint that is SO(2) - q_pin[joint.idx_q()] = std::cos(q[i]); - q_pin[joint.idx_q() + 1] = std::sin(q[i]); - } else { - q_pin[joint.idx_q()] = q[i]; - } - } - - pinocchio::forwardKinematics(model_, data_, q_pin); - pinocchio::updateFramePlacements(model_, data_); - - auto current_pose = data_.oMf[end_effector_frame_id]; - auto current_quaternion = Eigen::Quaterniond(current_pose.rotation()); - - // Decide whether to publish the pose or not - publish_elapsed_ = publish_elapsed_ + period; - bool should_publish = - (publish_elapsed_ >= publish_interval_) || (publish_interval_.nanoseconds() == 0); - if (should_publish && realtime_pose_publisher_) { - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = time; - pose_msg.header.frame_id = params_.base_frame; - pose_msg.pose.position.x = current_pose.translation()[0]; - pose_msg.pose.position.y = current_pose.translation()[1]; - pose_msg.pose.position.z = current_pose.translation()[2]; - pose_msg.pose.orientation.x = current_quaternion.x(); - pose_msg.pose.orientation.y = current_quaternion.y(); - pose_msg.pose.orientation.z = current_quaternion.z(); - pose_msg.pose.orientation.w = current_quaternion.w(); - -#if REALTIME_TOOLS_NEW_API - if (realtime_pose_publisher_->try_publish(pose_msg)) { -#else - if (realtime_pose_publisher_->trylock()) { - realtime_pose_publisher_->msg_ = pose_msg; - realtime_pose_publisher_->unlockAndPublish(); -#endif - publish_elapsed_ = publish_elapsed_ - publish_interval_; - // clamp to publish only 1 time even if missed multiple intervals - publish_elapsed_ = std::min(publish_elapsed_, publish_interval_); - } - } - - return controller_interface::return_type::OK; -} - -CallbackReturn PoseBroadcaster::on_init() { - // Initialize parameters - params_listener_ = std::make_shared(get_node()); - params_listener_->refresh_dynamic_parameters(); - params_ = params_listener_->get_params(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - auto parameters_client = - std::make_shared(get_node(), "robot_state_publisher"); - parameters_client->wait_for_service(); - - auto future = parameters_client->get_parameters({"robot_description"}); - auto result = future.get(); - - std::string robot_description_; - if (!result.empty()) { - robot_description_ = result[0].value_to_string(); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter."); - return CallbackReturn::ERROR; - } - - pinocchio::Model raw_model_; - pinocchio::urdf::buildModelFromXML(robot_description_, raw_model_); - - RCLCPP_INFO(get_node()->get_logger(), "Checking available joints in model:"); - for (int joint_id = 0; joint_id < raw_model_.njoints; joint_id++) { - RCLCPP_INFO_STREAM( - get_node()->get_logger(), - "Joint " << joint_id << " with name " << raw_model_.names[joint_id] << " is of type " - << raw_model_.joints[joint_id].shortname()); - } - - // First we check that the passed joints exist in the kineatic tree - for (auto & joint : params_.joints) { - if (!raw_model_.existJointName(joint)) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "Failed to configure because " - << joint - << " is not part of the kinematic tree but it has been passed in the parameters."); - return CallbackReturn::ERROR; - } - } - RCLCPP_INFO( - get_node()->get_logger(), - "All joints passed in the parameters exist in the kinematic tree of the URDF."); - RCLCPP_INFO_STREAM( - get_node()->get_logger(), "Removing the rest of the joints that are not used: "); - // Now we fix all joints that are not referenced in the tree - std::vector list_of_joints_to_lock_by_id; - for (auto & joint : raw_model_.names) { - if ( - std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() && - joint != "universe") { - RCLCPP_INFO_STREAM( - get_node()->get_logger(), "Joint " << joint << " is not used, removing it from the model."); - list_of_joints_to_lock_by_id.push_back(raw_model_.getJointId(joint)); - } - } - - Eigen::VectorXd q_locked = Eigen::VectorXd::Zero(raw_model_.nq); - model_ = pinocchio::buildReducedModel(raw_model_, list_of_joints_to_lock_by_id, q_locked); - data_ = pinocchio::Data(model_); - - for (int joint_id = 0; joint_id < model_.njoints; joint_id++) { - if (model_.names[joint_id] == "universe") { - continue; - } - if (!allowed_joint_types.count(model_.joints[joint_id].shortname())) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "Joint type " << model_.joints[joint_id].shortname() << " is unsupported (" - << model_.names[joint_id] - << "), only revolute/continous like joints can be used."); - return CallbackReturn::ERROR; - } - } - - if (!model_.existFrame(params_.end_effector_frame)) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "end_effector_frame '" << params_.end_effector_frame - << "' is not present in the robot model. Refusing to configure: " - "activating with an invalid frame results in undefined behavior " - "(out-of-bounds access into pinocchio::Data)."); - return CallbackReturn::ERROR; - } - end_effector_frame_id = model_.getFrameId(params_.end_effector_frame); - q = Eigen::VectorXd::Zero(model_.nv); - - pose_publisher_ = get_node()->create_publisher( - "current_pose", rclcpp::SystemDefaultsQoS()); - realtime_pose_publisher_ = - std::make_shared>( - pose_publisher_); - - if (params_.publish_frequency > 0.0) { - publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); - } else { - publish_interval_ = rclcpp::Duration(0, 0); // publish every cycle - } - - return CallbackReturn::SUCCESS; -} - -CallbackReturn PoseBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - // reset publish time accumulation - publish_elapsed_ = rclcpp::Duration(0, 0); - return CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn -PoseBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - return CallbackReturn::SUCCESS; -} - -} // namespace crisp_controllers -#include "pluginlib/class_list_macros.hpp" -// NOLINTNEXTLINE -PLUGINLIB_EXPORT_CLASS( - crisp_controllers::PoseBroadcaster, controller_interface::ControllerInterface) diff --git a/src/pose_broadcaster.yaml b/src/pose_broadcaster.yaml deleted file mode 100644 index 3c687d6..0000000 --- a/src/pose_broadcaster.yaml +++ /dev/null @@ -1,15 +0,0 @@ -pose_broadcaster: - joints: - type: string_array - default_value: [] - description: "Names of the joints" - end_effector_frame: - type: string - description: "Name of the end-effector frame" - base_frame: - type: string - description: "Name of the base frame" - publish_frequency: - type: double - default_value: 250.0 - description: "Publishing frequency in Hz (0.0 means no throttling)" diff --git a/src/state_broadcaster.cpp b/src/state_broadcaster.cpp new file mode 100644 index 0000000..ca2cd2d --- /dev/null +++ b/src/state_broadcaster.cpp @@ -0,0 +1,724 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include // NOLINT(build/include_order) + +#include +#include +#include "crisp_controllers/utils/ros2_version.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace crisp_controllers { + +controller_interface::InterfaceConfiguration +StateBroadcaster::command_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::NONE; + return config; +} + +controller_interface::InterfaceConfiguration +StateBroadcaster::state_interface_configuration() const { + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::ALL; + return config; +} + +controller_interface::return_type +StateBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & period) { + + // Handle position-interface-based broadcasting + read_position_interfaces(); + update_pinocchio_positions(); + pinocchio::forwardKinematics(model_, data_, q_pin_); + pinocchio::updateFramePlacements(model_, data_); + + const auto & current_pose = data_.oMf[end_effector_frame_id_]; + const Eigen::Quaterniond current_quaternion(current_pose.rotation()); + + if (should_publish(period, pose_timer_) && realtime_pose_publisher_) { + pose_msg_.header.stamp = time; + pose_msg_.pose.position.x = current_pose.translation()[0]; + pose_msg_.pose.position.y = current_pose.translation()[1]; + pose_msg_.pose.position.z = current_pose.translation()[2]; + pose_msg_.pose.orientation.x = current_quaternion.x(); + pose_msg_.pose.orientation.y = current_quaternion.y(); + pose_msg_.pose.orientation.z = current_quaternion.z(); + pose_msg_.pose.orientation.w = current_quaternion.w(); + +#if REALTIME_TOOLS_NEW_API + realtime_pose_publisher_->try_publish(pose_msg_); +#else + if (realtime_pose_publisher_->trylock()) { + realtime_pose_publisher_->msg_ = pose_msg_; + realtime_pose_publisher_->unlockAndPublish(); + } +#endif + } + + for (size_t i = 0; i < params_.joints.size(); ++i) { + joint_state_msg_.position[i] = q_[i]; + } + + // Handle velocity-interface-based broadcasting + if (has_velocity_interfaces_) { + read_velocity_interfaces(); + pinocchio::forwardKinematics(model_, data_, q_pin_, dq_); + pinocchio::updateFramePlacements(model_, data_); + + const auto current_velocity = + pinocchio::getFrameVelocity(model_, data_, end_effector_frame_id_, twist_reference_frame_); + if (should_publish(period, twist_timer_) && realtime_twist_publisher_) { + twist_msg_.header.stamp = time; + twist_msg_.twist.linear.x = current_velocity.linear()[0]; + twist_msg_.twist.linear.y = current_velocity.linear()[1]; + twist_msg_.twist.linear.z = current_velocity.linear()[2]; + twist_msg_.twist.angular.x = current_velocity.angular()[0]; + twist_msg_.twist.angular.y = current_velocity.angular()[1]; + twist_msg_.twist.angular.z = current_velocity.angular()[2]; + +#if REALTIME_TOOLS_NEW_API + realtime_twist_publisher_->try_publish(twist_msg_); +#else + if (realtime_twist_publisher_->trylock()) { + realtime_twist_publisher_->msg_ = twist_msg_; + realtime_twist_publisher_->unlockAndPublish(); + } +#endif + } + + for (size_t i = 0; i < params_.joints.size(); ++i) { + joint_state_msg_.velocity[i] = dq_[joint_v_indices_[i]]; + } + } + + // Handle effort-interface-based broadcasting + if (has_effort_interfaces_) { + read_effort_interfaces(); + for (size_t i = 0; i < params_.joints.size(); ++i) { + joint_state_msg_.effort[i] = tau_measured_[joint_v_indices_[i]]; + } + + compute_wrench(tau_measured_, raw_wrench_reference_frame_, raw_wrench_); + if (should_publish(period, raw_wrench_timer_) && realtime_raw_wrench_publisher_) { + raw_wrench_msg_.header.stamp = time; + raw_wrench_msg_.wrench.force.x = raw_wrench_[0]; + raw_wrench_msg_.wrench.force.y = raw_wrench_[1]; + raw_wrench_msg_.wrench.force.z = raw_wrench_[2]; + raw_wrench_msg_.wrench.torque.x = raw_wrench_[3]; + raw_wrench_msg_.wrench.torque.y = raw_wrench_[4]; + raw_wrench_msg_.wrench.torque.z = raw_wrench_[5]; + +#if REALTIME_TOOLS_NEW_API + realtime_raw_wrench_publisher_->try_publish(raw_wrench_msg_); +#else + if (realtime_raw_wrench_publisher_->trylock()) { + realtime_raw_wrench_publisher_->msg_ = raw_wrench_msg_; + realtime_raw_wrench_publisher_->unlockAndPublish(); + } +#endif + } + } + + // External joint effort and external wrench derivation require velocity available + if (has_velocity_interfaces_ && has_effort_interfaces_) { + if (params_.use_coriolis_compensation) { + pinocchio::computeAllTerms(model_, data_, q_pin_, dq_); + tau_coriolis_.noalias() = + pinocchio::computeCoriolisMatrix(model_, data_, q_pin_, dq_) * dq_; + } else { + tau_coriolis_.setZero(); + } + + if (params_.use_gravity_compensation) { + tau_gravity_.noalias() = pinocchio::computeGeneralizedGravity(model_, data_, q_pin_); + } else { + tau_gravity_.setZero(); + } + + if (params_.use_inertial_compensation) { + const double period_seconds = period.seconds(); + if (!has_previous_velocity_) { + dq_previous_ = dq_; + tau_inertia_.setZero(); + has_previous_velocity_ = true; + } else if (period_seconds > 0.0) { + ddq_estimated_.noalias() = dq_ - dq_previous_; + ddq_estimated_ /= period_seconds; + for (Eigen::Index i = 0; i < ddq_filtered_.size(); ++i) { + ddq_filtered_[i] = exponential_moving_average( + ddq_filtered_[i], ddq_estimated_[i], params_.acceleration_filter_alpha); + } + + pinocchio::crba(model_, data_, q_pin_); + data_.M.triangularView() = + data_.M.transpose().triangularView(); + tau_inertia_.noalias() = data_.M * ddq_filtered_; + dq_previous_ = dq_; + } else { + tau_inertia_.setZero(); + dq_previous_ = dq_; + } + } else { + tau_inertia_.setZero(); + has_previous_velocity_ = false; + } + + tau_residual_.noalias() = tau_measured_; + tau_residual_ -= tau_coriolis_; + tau_residual_ -= tau_gravity_; + tau_residual_ -= tau_inertia_; + update_external_effort_message(); + + if (should_publish(period, external_effort_timer_) && realtime_external_effort_publisher_) { +#if REALTIME_TOOLS_NEW_API + realtime_external_effort_publisher_->try_publish(external_effort_msg_); +#else + if (realtime_external_effort_publisher_->trylock()) { + realtime_external_effort_publisher_->msg_ = external_effort_msg_; + realtime_external_effort_publisher_->unlockAndPublish(); + } +#endif + } + + compute_wrench(tau_residual_, external_wrench_reference_frame_, external_wrench_); + if (should_publish(period, external_wrench_timer_) && realtime_external_wrench_publisher_) { + external_wrench_msg_.header.stamp = time; + external_wrench_msg_.wrench.force.x = external_wrench_[0]; + external_wrench_msg_.wrench.force.y = external_wrench_[1]; + external_wrench_msg_.wrench.force.z = external_wrench_[2]; + external_wrench_msg_.wrench.torque.x = external_wrench_[3]; + external_wrench_msg_.wrench.torque.y = external_wrench_[4]; + external_wrench_msg_.wrench.torque.z = external_wrench_[5]; + +#if REALTIME_TOOLS_NEW_API + realtime_external_wrench_publisher_->try_publish(external_wrench_msg_); +#else + if (realtime_external_wrench_publisher_->trylock()) { + realtime_external_wrench_publisher_->msg_ = external_wrench_msg_; + realtime_external_wrench_publisher_->unlockAndPublish(); + } +#endif + } + } + + if (should_publish(period, joint_state_timer_) && realtime_joint_state_publisher_) { + joint_state_msg_.header.stamp = time; +#if REALTIME_TOOLS_NEW_API + realtime_joint_state_publisher_->try_publish(joint_state_msg_); +#else + if (realtime_joint_state_publisher_->trylock()) { + realtime_joint_state_publisher_->msg_ = joint_state_msg_; + realtime_joint_state_publisher_->unlockAndPublish(); + } +#endif + } + + return controller_interface::return_type::OK; +} + +CallbackReturn StateBroadcaster::on_init() { + params_listener_ = std::make_shared(get_node()); + params_listener_->refresh_dynamic_parameters(); + params_ = params_listener_->get_params(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn +StateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!validate_common_parameters() || !build_model() || !cache_joint_model_indices()) { + return CallbackReturn::ERROR; + } + + std::string raw_wrench_frame; + std::string external_wrench_frame; + if ( + !configure_reference_frame( + "twist", params_.twist.reference_frame, twist_reference_frame_, twist_msg_.header.frame_id) || + !configure_reference_frame( + "wrench.raw", params_.wrench.raw.reference_frame, raw_wrench_reference_frame_, + raw_wrench_frame) || + !configure_reference_frame( + "wrench.external", params_.wrench.external.reference_frame, external_wrench_reference_frame_, + external_wrench_frame)) { + return CallbackReturn::ERROR; + } + + pose_msg_.header.frame_id = + params_.pose.frame.empty() ? params_.base_frame : params_.pose.frame; + twist_msg_.header.frame_id = + params_.twist.frame.empty() ? twist_msg_.header.frame_id : params_.twist.frame; + raw_wrench_msg_.header.frame_id = + params_.wrench.raw.frame.empty() ? raw_wrench_frame : params_.wrench.raw.frame; + external_wrench_msg_.header.frame_id = + params_.wrench.external.frame.empty() ? external_wrench_frame : params_.wrench.external.frame; + + if ( + !configure_publish_interval("pose", params_.pose.publish_frequency, pose_timer_) || + !configure_publish_interval("twist", params_.twist.publish_frequency, twist_timer_) || + !configure_publish_interval( + "wrench.raw", params_.wrench.raw.publish_frequency, raw_wrench_timer_) || + !configure_publish_interval( + "wrench.external", params_.wrench.external.publish_frequency, external_wrench_timer_) || + !configure_publish_interval( + "effort.external", params_.effort.external.publish_frequency, external_effort_timer_) || + !configure_publish_interval( + "joint_states", params_.joint_states.publish_frequency, joint_state_timer_)) { + return CallbackReturn::ERROR; + } + + pose_publisher_ = get_node()->create_publisher( + params_.pose.topic, rclcpp::SystemDefaultsQoS()); + realtime_pose_publisher_ = + std::make_shared>( + pose_publisher_); + + twist_publisher_ = get_node()->create_publisher( + params_.twist.topic, rclcpp::SystemDefaultsQoS()); + realtime_twist_publisher_ = + std::make_shared>( + twist_publisher_); + + raw_wrench_publisher_ = get_node()->create_publisher( + params_.wrench.raw.topic, rclcpp::SystemDefaultsQoS()); + realtime_raw_wrench_publisher_ = + std::make_shared>( + raw_wrench_publisher_); + + external_wrench_publisher_ = get_node()->create_publisher( + params_.wrench.external.topic, rclcpp::SystemDefaultsQoS()); + realtime_external_wrench_publisher_ = + std::make_shared>( + external_wrench_publisher_); + + external_effort_publisher_ = get_node()->create_publisher( + params_.effort.external.topic, rclcpp::SystemDefaultsQoS()); + realtime_external_effort_publisher_ = + std::make_shared>( + external_effort_publisher_); + + joint_state_publisher_ = get_node()->create_publisher( + params_.joint_states.topic, rclcpp::SystemDefaultsQoS()); + realtime_joint_state_publisher_ = + std::make_shared>( + joint_state_publisher_); + +#if !REALTIME_TOOLS_NEW_API + realtime_pose_publisher_->msg_.header.frame_id = pose_msg_.header.frame_id; + realtime_twist_publisher_->msg_.header.frame_id = twist_msg_.header.frame_id; + realtime_raw_wrench_publisher_->msg_.header.frame_id = raw_wrench_msg_.header.frame_id; + realtime_external_wrench_publisher_->msg_.header.frame_id = + external_wrench_msg_.header.frame_id; +#endif + + return CallbackReturn::SUCCESS; +} + +CallbackReturn StateBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!cache_state_interface_indices()) { + return CallbackReturn::ERROR; + } + + pose_timer_.elapsed = rclcpp::Duration(0, 0); + twist_timer_.elapsed = rclcpp::Duration(0, 0); + raw_wrench_timer_.elapsed = rclcpp::Duration(0, 0); + external_wrench_timer_.elapsed = rclcpp::Duration(0, 0); + external_effort_timer_.elapsed = rclcpp::Duration(0, 0); + joint_state_timer_.elapsed = rclcpp::Duration(0, 0); + has_previous_velocity_ = false; + ddq_estimated_.setZero(); + ddq_filtered_.setZero(); + tau_inertia_.setZero(); + + joint_state_msg_.name = params_.joints; + joint_state_msg_.position.assign(params_.joints.size(), 0.0); + joint_state_msg_.velocity.clear(); + joint_state_msg_.effort.clear(); + if (has_velocity_interfaces_) { + joint_state_msg_.velocity.assign(params_.joints.size(), 0.0); + } + if (has_effort_interfaces_) { + joint_state_msg_.effort.assign(params_.joints.size(), 0.0); + } + + external_effort_msg_.data.assign(params_.joints.size(), 0.0); + +#if !REALTIME_TOOLS_NEW_API + realtime_joint_state_publisher_->msg_ = joint_state_msg_; + realtime_external_effort_publisher_->msg_ = external_effort_msg_; +#endif + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +StateBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + return CallbackReturn::SUCCESS; +} + +bool StateBroadcaster::configure_reference_frame( + const std::string & output_name, const std::string & reference_frame, + pinocchio::ReferenceFrame & parsed_reference_frame, std::string & published_frame) const { + if (reference_frame == "local") { + parsed_reference_frame = pinocchio::ReferenceFrame::LOCAL; + published_frame = params_.end_effector_frame; + return true; + } + if (reference_frame == "local_world_aligned") { + if (params_.base_frame.empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because base_frame is empty for %s local_world_aligned output.", + output_name.c_str()); + return false; + } + parsed_reference_frame = pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED; + published_frame = params_.base_frame; + return true; + } + + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because %s reference_frame '%s' is unsupported.", output_name.c_str(), + reference_frame.c_str()); + return false; +} + +bool StateBroadcaster::configure_publish_interval( + const std::string & output_name, double publish_frequency, PublishTimer & timer) const { + if (publish_frequency < 0.0) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because %s publish_frequency must be non-negative.", + output_name.c_str()); + return false; + } + + timer.interval = publish_frequency > 0.0 ? rclcpp::Duration::from_seconds(1.0 / publish_frequency) + : rclcpp::Duration(0, 0); + return true; +} + +bool StateBroadcaster::validate_common_parameters() const { + if (params_.joints.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure because joints is empty."); + return false; + } + if (params_.end_effector_frame.empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to configure because end_effector_frame is empty."); + return false; + } + if (params_.base_frame.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure because base_frame is empty."); + return false; + } + + const std::array, 6> topics = {{ + {"pose.topic", params_.pose.topic}, + {"twist.topic", params_.twist.topic}, + {"wrench.raw.topic", params_.wrench.raw.topic}, + {"wrench.external.topic", params_.wrench.external.topic}, + {"effort.external.topic", params_.effort.external.topic}, + {"joint_states.topic", params_.joint_states.topic}, + }}; + for (const auto & [name, topic] : topics) { + if (topic.empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to configure because %s is empty.", name); + return false; + } + } + + return true; +} + +bool StateBroadcaster::build_model() { + constexpr auto robot_state_publisher_node = "robot_state_publisher"; + constexpr auto robot_description_parameter = "robot_description"; + constexpr auto parameter_service_timeout = std::chrono::seconds(2); + + auto parameters_client = + std::make_shared(get_node(), robot_state_publisher_node); + if (!parameters_client->wait_for_service(parameter_service_timeout)) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because %s did not provide its parameter service within 2 seconds; " + "cannot read %s.", + robot_state_publisher_node, robot_description_parameter); + return false; + } + + auto parameters_future = parameters_client->get_parameters({robot_description_parameter}); + if (parameters_future.wait_for(parameter_service_timeout) != std::future_status::ready) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because %s did not return %s within 2 seconds.", + robot_state_publisher_node, robot_description_parameter); + return false; + } + + const auto result = parameters_future.get(); + + std::string robot_description; + if ( + !result.empty() && result[0].get_name() == robot_description_parameter && + result[0].get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + robot_description = result[0].as_string(); + } else { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because %s did not return a string %s parameter.", + robot_state_publisher_node, robot_description_parameter); + return false; + } + + if (robot_description.empty()) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because %s returned an empty %s parameter.", + robot_state_publisher_node, robot_description_parameter); + return false; + } + + pinocchio::Model raw_model; + pinocchio::urdf::buildModelFromXML(robot_description, raw_model); + + for (const auto & joint : params_.joints) { + if (!raw_model.existJointName(joint)) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Failed to configure because " + << joint + << " is not part of the kinematic tree but it has been passed in the parameters."); + return false; + } + } + + std::vector list_of_joints_to_lock_by_id; + for (const auto & joint : raw_model.names) { + if ( + std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() && + joint != "universe") { + list_of_joints_to_lock_by_id.push_back(raw_model.getJointId(joint)); + } + } + + Eigen::VectorXd q_locked = Eigen::VectorXd::Zero(raw_model.nq); + model_ = pinocchio::buildReducedModel(raw_model, list_of_joints_to_lock_by_id, q_locked); + data_ = pinocchio::Data(model_); + + for (int joint_id = 0; joint_id < model_.njoints; joint_id++) { + if (model_.names[joint_id] == "universe") { + continue; + } + if (!allowed_joint_types_.count(model_.joints[joint_id].shortname())) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Joint type " << model_.joints[joint_id].shortname() << " is unsupported (" + << model_.names[joint_id] + << "), only revolute/continuous-like joints can be used."); + return false; + } + } + + if (!model_.existFrame(params_.end_effector_frame)) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure because end_effector_frame '%s' is not part of the reduced model.", + params_.end_effector_frame.c_str()); + return false; + } + + end_effector_frame_id_ = model_.getFrameId(params_.end_effector_frame); + + q_ = Eigen::VectorXd::Zero(static_cast(params_.joints.size())); + q_pin_ = Eigen::VectorXd::Zero(model_.nq); + dq_ = Eigen::VectorXd::Zero(model_.nv); + dq_previous_ = Eigen::VectorXd::Zero(model_.nv); + ddq_estimated_ = Eigen::VectorXd::Zero(model_.nv); + ddq_filtered_ = Eigen::VectorXd::Zero(model_.nv); + tau_measured_ = Eigen::VectorXd::Zero(model_.nv); + tau_coriolis_ = Eigen::VectorXd::Zero(model_.nv); + tau_gravity_ = Eigen::VectorXd::Zero(model_.nv); + tau_inertia_ = Eigen::VectorXd::Zero(model_.nv); + tau_residual_ = Eigen::VectorXd::Zero(model_.nv); + J_ = pinocchio::Data::Matrix6x::Zero(6, model_.nv); + wrench_system_.setZero(); + wrench_rhs_.setZero(); + raw_wrench_.setZero(); + external_wrench_.setZero(); + + return true; +} + +bool StateBroadcaster::cache_joint_model_indices() { + joint_q_indices_.clear(); + joint_v_indices_.clear(); + joint_is_continuous_.clear(); + joint_q_indices_.reserve(params_.joints.size()); + joint_v_indices_.reserve(params_.joints.size()); + joint_is_continuous_.reserve(params_.joints.size()); + + for (const auto & joint_name : params_.joints) { + const auto joint_id = model_.getJointId(joint_name); + const auto & joint = model_.joints[joint_id]; + joint_q_indices_.push_back(joint.idx_q()); + joint_v_indices_.push_back(joint.idx_v()); + joint_is_continuous_.push_back(continuous_joint_types_.count(joint.shortname()) > 0); + } + + return true; +} + +bool StateBroadcaster::cache_state_interface_indices() { + position_interface_indices_.assign(params_.joints.size(), kMissingInterface); + velocity_interface_indices_.assign(params_.joints.size(), kMissingInterface); + effort_interface_indices_.assign(params_.joints.size(), kMissingInterface); + + for (size_t interface_index = 0; interface_index < state_interfaces_.size(); ++interface_index) { + const auto joint_name = state_interfaces_[interface_index].get_prefix_name(); + const auto interface_name = state_interfaces_[interface_index].get_interface_name(); + for (size_t joint_index = 0; joint_index < params_.joints.size(); ++joint_index) { + if (joint_name != params_.joints[joint_index]) { + continue; + } + if (interface_name == "position") { + position_interface_indices_[joint_index] = interface_index; + } else if (interface_name == "velocity") { + velocity_interface_indices_[joint_index] = interface_index; + } else if (interface_name == "effort") { + effort_interface_indices_[joint_index] = interface_index; + } + } + } + + for (size_t i = 0; i < params_.joints.size(); ++i) { + if (position_interface_indices_[i] == kMissingInterface) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to activate because joint '%s' does not provide a position state interface.", + params_.joints[i].c_str()); + return false; + } + } + + has_velocity_interfaces_ = std::all_of( + velocity_interface_indices_.begin(), velocity_interface_indices_.end(), + [](size_t index) { return index != kMissingInterface; }); + has_effort_interfaces_ = std::all_of( + effort_interface_indices_.begin(), effort_interface_indices_.end(), + [](size_t index) { return index != kMissingInterface; }); + + if (!has_velocity_interfaces_) { + RCLCPP_INFO( + get_node()->get_logger(), + "StateBroadcaster activated without a complete velocity interface group; twist and external " + "effort outputs are disabled."); + } + if (!has_effort_interfaces_) { + RCLCPP_INFO( + get_node()->get_logger(), + "StateBroadcaster activated without a complete effort interface group; wrench and external " + "effort outputs are disabled."); + } + + return true; +} + +void StateBroadcaster::read_position_interfaces() { + for (size_t i = 0; i < params_.joints.size(); ++i) { +#if ROS2_VERSION_ABOVE_HUMBLE + q_[i] = state_interfaces_[position_interface_indices_[i]].get_optional().value_or(q_[i]); +#else + q_[i] = state_interfaces_[position_interface_indices_[i]].get_value(); +#endif + } +} + +void StateBroadcaster::read_velocity_interfaces() { + for (size_t i = 0; i < params_.joints.size(); ++i) { +#if ROS2_VERSION_ABOVE_HUMBLE + dq_[joint_v_indices_[i]] = state_interfaces_[velocity_interface_indices_[i]] + .get_optional() + .value_or(dq_[joint_v_indices_[i]]); +#else + dq_[joint_v_indices_[i]] = state_interfaces_[velocity_interface_indices_[i]].get_value(); +#endif + } +} + +void StateBroadcaster::read_effort_interfaces() { + for (size_t i = 0; i < params_.joints.size(); ++i) { +#if ROS2_VERSION_ABOVE_HUMBLE + tau_measured_[joint_v_indices_[i]] = state_interfaces_[effort_interface_indices_[i]] + .get_optional() + .value_or(tau_measured_[joint_v_indices_[i]]); +#else + tau_measured_[joint_v_indices_[i]] = + state_interfaces_[effort_interface_indices_[i]].get_value(); +#endif + } +} + +void StateBroadcaster::update_pinocchio_positions() { + q_pin_.setZero(); + for (size_t i = 0; i < params_.joints.size(); ++i) { + if (joint_is_continuous_[i]) { + q_pin_[joint_q_indices_[i]] = std::cos(q_[i]); + q_pin_[joint_q_indices_[i] + 1] = std::sin(q_[i]); + } else { + q_pin_[joint_q_indices_[i]] = q_[i]; + } + } +} + +void StateBroadcaster::compute_wrench( + const Eigen::VectorXd & effort, pinocchio::ReferenceFrame reference_frame, + Eigen::Matrix & wrench) { + J_.setZero(); + pinocchio::computeFrameJacobian( + model_, data_, q_pin_, end_effector_frame_id_, reference_frame, J_); + wrench_system_.noalias() = J_ * J_.transpose(); + wrench_system_.diagonal().array() += params_.wrench.regularization; + wrench_rhs_.noalias() = J_ * effort; + wrench = wrench_system_.ldlt().solve(wrench_rhs_); +} + +void StateBroadcaster::update_external_effort_message() { + for (size_t i = 0; i < params_.joints.size(); ++i) { + external_effort_msg_.data[i] = tau_residual_[joint_v_indices_[i]]; + } +} + +bool StateBroadcaster::should_publish(const rclcpp::Duration & period, PublishTimer & timer) { + timer.elapsed = timer.elapsed + period; + const bool should_publish = + (timer.elapsed >= timer.interval) || (timer.interval.nanoseconds() == 0); + if (should_publish) { + timer.elapsed = timer.elapsed - timer.interval; + timer.elapsed = std::min(timer.elapsed, timer.interval); + } + return should_publish; +} + +} // namespace crisp_controllers + +#include "pluginlib/class_list_macros.hpp" +// NOLINTNEXTLINE +PLUGINLIB_EXPORT_CLASS( + crisp_controllers::StateBroadcaster, controller_interface::ControllerInterface) diff --git a/src/state_broadcaster.yaml b/src/state_broadcaster.yaml new file mode 100644 index 0000000..eef5822 --- /dev/null +++ b/src/state_broadcaster.yaml @@ -0,0 +1,125 @@ +state_broadcaster: + joints: + type: string_array + default_value: [] + description: "Names of the joints" + end_effector_frame: + type: string + description: "Name of the end-effector frame" + base_frame: + type: string + description: "Name of the base frame" + use_gravity_compensation: + type: bool + default_value: true + description: "Subtract model gravity torques from the external effort residual" + use_coriolis_compensation: + type: bool + default_value: true + description: "Subtract model Coriolis torques from the external effort residual" + use_inertial_compensation: + type: bool + default_value: true + description: "Subtract estimated model inertial torques from the external effort residual" + acceleration_filter_alpha: + type: double + default_value: 0.80 + description: "EMA smoothing factor for estimated joint acceleration; higher values smooth more" + validation: + bounds<>: [0.0, 1.0] + pose: + topic: + type: string + default_value: "current_pose" + description: "Topic name for publishing the current pose (geometry_msgs/PoseStamped)" + publish_frequency: + type: double + default_value: 500.0 + description: "Publishing frequency in Hz (0.0 means no throttling)" + frame: + type: string + default_value: "" + description: "Frame id for the pose output; empty uses base_frame" + twist: + topic: + type: string + default_value: "current_twist" + description: "Topic name for publishing the current twist (geometry_msgs/TwistStamped)" + publish_frequency: + type: double + default_value: 500.0 + description: "Publishing frequency in Hz (0.0 means no throttling)" + frame: + type: string + default_value: "" + description: "Frame id for the twist output; empty uses end_effector_frame" + reference_frame: + type: string + default_value: "local" + description: "Frame convention for the published twist: local or local_world_aligned" + validation: + one_of<>: [["local", "local_world_aligned"]] + wrench: + regularization: + type: double + default_value: 0.000001 + description: "Damping used for the wrench least-squares solve" + validation: + bounds<>: [0.000001, 0.01] + raw: + topic: + type: string + default_value: "current_wrench" + description: "Topic name for publishing the measured-effort wrench (geometry_msgs/WrenchStamped)" + publish_frequency: + type: double + default_value: 500.0 + description: "Publishing frequency in Hz (0.0 means no throttling)" + frame: + type: string + default_value: "" + description: "Frame id for the raw wrench output; empty follows reference_frame" + reference_frame: + type: string + default_value: "local_world_aligned" + description: "Frame convention for the published raw wrench: local or local_world_aligned" + validation: + one_of<>: [["local", "local_world_aligned"]] + external: + topic: + type: string + default_value: "external_wrench" + description: "Topic name for publishing the residual external wrench (geometry_msgs/WrenchStamped)" + publish_frequency: + type: double + default_value: 500.0 + description: "Publishing frequency in Hz (0.0 means no throttling)" + frame: + type: string + default_value: "" + description: "Frame id for the external wrench output; empty follows reference_frame" + reference_frame: + type: string + default_value: "local_world_aligned" + description: "Frame convention for the published external wrench: local or local_world_aligned" + validation: + one_of<>: [["local", "local_world_aligned"]] + effort: + external: + topic: + type: string + default_value: "external_effort" + description: "Topic name for publishing residual torques (std_msgs/Float64MultiArray)" + publish_frequency: + type: double + default_value: 500.0 + description: "Publishing frequency in Hz (0.0 means no throttling)" + joint_states: + topic: + type: string + default_value: "joint_states" + description: "Topic name for publishing joint states (sensor_msgs/JointState)" + publish_frequency: + type: double + default_value: 1000.0 + description: "Publishing frequency in Hz (0.0 means no throttling)" diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp deleted file mode 100644 index 9e242d2..0000000 --- a/src/twist_broadcaster.cpp +++ /dev/null @@ -1,231 +0,0 @@ -#include -#include -#include -#include - -#include // NOLINT(build/include_order) -#include -#include "crisp_controllers/utils/ros2_version.hpp" - -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; - -namespace crisp_controllers { - -controller_interface::InterfaceConfiguration -TwistBroadcaster::command_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::NONE; - return config; -} - -controller_interface::InterfaceConfiguration -TwistBroadcaster::state_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto & joint_name : params_.joints) { - config.names.push_back(joint_name + "/position"); - config.names.push_back(joint_name + "/velocity"); - } - return config; -} - -controller_interface::return_type -TwistBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & period) { - size_t num_joints = params_.joints.size(); - Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq); - Eigen::VectorXd q_dot_pin = Eigen::VectorXd::Zero(model_.nv); - - for (size_t i = 0; i < num_joints; i++) { - auto joint_name = params_.joints[i]; - auto joint_id = model_.getJointId(joint_name); - auto joint = model_.joints[joint_id]; - -#if ROS2_VERSION_ABOVE_HUMBLE - q[i] = state_interfaces_[i * 2].get_optional().value_or(q[i]); - q_dot[i] = state_interfaces_[i * 2 + 1].get_optional().value_or(q_dot[i]); -#else - q[i] = state_interfaces_[i * 2].get_value(); - q_dot[i] = state_interfaces_[i * 2 + 1].get_value(); -#endif - - if (continous_joint_types.count( - joint.shortname())) { // Then we are handling a continous joint that is SO(2) - q_pin[joint.idx_q()] = std::cos(q[i]); - q_pin[joint.idx_q() + 1] = std::sin(q[i]); - q_dot_pin[joint.idx_v()] = q_dot[i]; - } else { - q_pin[joint.idx_q()] = q[i]; - q_dot_pin[joint.idx_v()] = q_dot[i]; - } - } - - pinocchio::forwardKinematics(model_, data_, q_pin, q_dot_pin); - pinocchio::updateFramePlacements(model_, data_); - - auto current_velocity = pinocchio::getFrameVelocity(model_, data_, end_effector_frame_id); - - // Decide whether to publish the twist or not - publish_elapsed_ = publish_elapsed_ + period; - bool should_publish = - (publish_elapsed_ >= publish_interval_) || (publish_interval_.nanoseconds() == 0); - - if (should_publish && realtime_twist_publisher_) { - geometry_msgs::msg::TwistStamped twist_msg; - twist_msg.header.stamp = time; - twist_msg.header.frame_id = params_.end_effector_frame; - twist_msg.twist.linear.x = current_velocity.linear()[0]; - twist_msg.twist.linear.y = current_velocity.linear()[1]; - twist_msg.twist.linear.z = current_velocity.linear()[2]; - twist_msg.twist.angular.x = current_velocity.angular()[0]; - twist_msg.twist.angular.y = current_velocity.angular()[1]; - twist_msg.twist.angular.z = current_velocity.angular()[2]; - -#if REALTIME_TOOLS_NEW_API - if (realtime_twist_publisher_->try_publish(twist_msg)) { -#else - if (realtime_twist_publisher_->trylock()) { - realtime_twist_publisher_->msg_ = twist_msg; - realtime_twist_publisher_->unlockAndPublish(); -#endif - publish_elapsed_ = publish_elapsed_ - publish_interval_; - // clamp to publish only 1 time even if missed multiple intervals - publish_elapsed_ = std::min(publish_elapsed_, publish_interval_); - } - } - - return controller_interface::return_type::OK; -} - -CallbackReturn TwistBroadcaster::on_init() { - // Initialize parameters - params_listener_ = std::make_shared(get_node()); - params_listener_->refresh_dynamic_parameters(); - params_ = params_listener_->get_params(); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn TwistBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - auto parameters_client = - std::make_shared(get_node(), "robot_state_publisher"); - parameters_client->wait_for_service(); - - auto future = parameters_client->get_parameters({"robot_description"}); - auto result = future.get(); - - std::string robot_description_; - if (!result.empty()) { - robot_description_ = result[0].value_to_string(); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter."); - return CallbackReturn::ERROR; - } - - pinocchio::Model raw_model_; - pinocchio::urdf::buildModelFromXML(robot_description_, raw_model_); - - RCLCPP_INFO(get_node()->get_logger(), "Checking available joints in model:"); - for (int joint_id = 0; joint_id < raw_model_.njoints; joint_id++) { - RCLCPP_INFO_STREAM( - get_node()->get_logger(), - "Joint " << joint_id << " with name " << raw_model_.names[joint_id] << " is of type " - << raw_model_.joints[joint_id].shortname()); - } - - // First we check that the passed joints exist in the kineatic tree - for (auto & joint : params_.joints) { - if (!raw_model_.existJointName(joint)) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "Failed to configure because " - << joint - << " is not part of the kinematic tree but it has been passed in the parameters."); - return CallbackReturn::ERROR; - } - } - RCLCPP_INFO( - get_node()->get_logger(), - "All joints passed in the parameters exist in the kinematic tree of the URDF."); - RCLCPP_INFO_STREAM( - get_node()->get_logger(), "Removing the rest of the joints that are not used: "); - // Now we fix all joints that are not referenced in the tree - std::vector list_of_joints_to_lock_by_id; - for (auto & joint : raw_model_.names) { - if ( - std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() && - joint != "universe") { - RCLCPP_INFO_STREAM( - get_node()->get_logger(), "Joint " << joint << " is not used, removing it from the model."); - list_of_joints_to_lock_by_id.push_back(raw_model_.getJointId(joint)); - } - } - - Eigen::VectorXd q_locked = Eigen::VectorXd::Zero(raw_model_.nq); - model_ = pinocchio::buildReducedModel(raw_model_, list_of_joints_to_lock_by_id, q_locked); - data_ = pinocchio::Data(model_); - - for (int joint_id = 0; joint_id < model_.njoints; joint_id++) { - if (model_.names[joint_id] == "universe") { - continue; - } - if (!allowed_joint_types.count(model_.joints[joint_id].shortname())) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "Joint type " << model_.joints[joint_id].shortname() << " is unsupported (" - << model_.names[joint_id] - << "), only revolute/continous like joints can be used."); - return CallbackReturn::ERROR; - } - } - - if (!model_.existFrame(params_.end_effector_frame)) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "end_effector_frame '" << params_.end_effector_frame - << "' is not present in the robot model. Refusing to configure: " - "activating with an invalid frame results in undefined behavior " - "(out-of-bounds access into pinocchio::Data)."); - return CallbackReturn::ERROR; - } - end_effector_frame_id = model_.getFrameId(params_.end_effector_frame); - q = Eigen::VectorXd::Zero(model_.nv); - q_dot = Eigen::VectorXd::Zero(model_.nv); - - twist_publisher_ = get_node()->create_publisher( - "current_twist", rclcpp::SystemDefaultsQoS()); - realtime_twist_publisher_ = - std::make_shared>( - twist_publisher_); - - if (params_.publish_frequency > 0.0) { - publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); - } else { - publish_interval_ = rclcpp::Duration(0, 0); // publish every cycle - } - - return CallbackReturn::SUCCESS; -} - -CallbackReturn TwistBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - // reset publish time accumulation - publish_elapsed_ = rclcpp::Duration(0, 0); - return CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn -TwistBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - return CallbackReturn::SUCCESS; -} - -} // namespace crisp_controllers -#include "pluginlib/class_list_macros.hpp" -// NOLINTNEXTLINE -PLUGINLIB_EXPORT_CLASS( - crisp_controllers::TwistBroadcaster, controller_interface::ControllerInterface) diff --git a/src/twist_broadcaster.yaml b/src/twist_broadcaster.yaml deleted file mode 100644 index a455667..0000000 --- a/src/twist_broadcaster.yaml +++ /dev/null @@ -1,15 +0,0 @@ -twist_broadcaster: - joints: - type: string_array - default_value: [] - description: "Names of the joints" - end_effector_frame: - type: string - description: "Name of the end-effector frame" - base_frame: - type: string - description: "Name of the base frame" - publish_frequency: - type: double - default_value: 250.0 - description: "Publishing frequency in Hz (0.0 means no throttling)" From 2b33ab777caf3d3e5d0d3ff39b8ba036a75e9e68 Mon Sep 17 00:00:00 2001 From: David Alexander Date: Wed, 24 Jun 2026 23:43:36 +0000 Subject: [PATCH 2/3] revert: undo effort publisher implementation (unneccessary) --- .../cartesian_controller.hpp | 14 ----- src/cartesian_controller.cpp | 58 ------------------- src/cartesian_controller.yaml | 12 +--- 3 files changed, 1 insertion(+), 83 deletions(-) diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 315f3f5..32df937 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include @@ -107,14 +106,6 @@ class CartesianController : public controller_interface::ControllerInterface { /** @brief Subscription for variable stiffness messages */ rclcpp::Subscription::SharedPtr stiffness_sub_; - /** @brief Publisher for commanded joint effort values */ - rclcpp::Publisher::SharedPtr effort_publisher_; - /** @brief Realtime publisher for commanded joint effort values */ - std::shared_ptr> - realtime_effort_publisher_; - /** @brief Pre-sized commanded effort message for realtime publication */ - std_msgs::msg::Float64MultiArray effort_msg_; - /** @brief Flag to indicate if multiple publishers detected */ bool multiple_publishers_detected_; @@ -283,11 +274,6 @@ class CartesianController : public controller_interface::ControllerInterface { /** @brief Final desired torque command */ Eigen::VectorXd tau_d; - /** @brief Time accumulated since the last effort publication */ - rclcpp::Duration publish_elapsed_{0, 0}; - /** @brief Effort publication interval */ - rclcpp::Duration publish_interval_{0, 0}; - /** @brief Inverse of the manipulator joint mass projected in Cartesian space (6x6) */ Eigen::Matrix Mx_inv = Eigen::Matrix::Zero(); /** @brief the manipulator joint mass projected in Cartesian space (6x6) */ diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 61a78eb..99118fb 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -226,30 +226,6 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & } } - publish_elapsed_ = publish_elapsed_ + period; - const bool should_publish = - realtime_effort_publisher_ && - ((publish_elapsed_ >= publish_interval_) || (publish_interval_.nanoseconds() == 0)); - if (should_publish) { -#if REALTIME_TOOLS_NEW_API - for (size_t i = 0; i < effort_msg_.data.size(); ++i) { - effort_msg_.data[i] = params_.stop_commands ? 0.0 : tau_d[i]; - } - if (realtime_effort_publisher_->try_publish(effort_msg_)) { -#else - if (realtime_effort_publisher_->trylock()) { - auto & effort_data = realtime_effort_publisher_->msg_.data; - for (size_t i = 0; i < effort_data.size(); ++i) { - effort_data[i] = params_.stop_commands ? 0.0 : tau_d[i]; - } - realtime_effort_publisher_->unlockAndPublish(); -#endif - publish_elapsed_ = publish_elapsed_ - publish_interval_; - // clamp to publish only 1 time even if missed multiple intervals - publish_elapsed_ = std::min(publish_elapsed_, publish_interval_); - } - } - tau_previous = tau_d; params_listener_->refresh_dynamic_parameters(); @@ -470,38 +446,6 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta tau_wrench = Eigen::VectorXd::Zero(model_.nv); tau_d = Eigen::VectorXd::Zero(model_.nv); - effort_publisher_.reset(); - realtime_effort_publisher_.reset(); - publish_elapsed_ = rclcpp::Duration(0, 0); - publish_interval_ = rclcpp::Duration(0, 0); - if (params_.effort_publisher.publish_frequency > 0.0) { - if (params_.effort_publisher.topic.empty()) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Failed to configure because effort_publisher.topic is empty."); - return CallbackReturn::ERROR; - } - effort_publisher_ = get_node()->create_publisher( - params_.effort_publisher.topic, rclcpp::SystemDefaultsQoS()); - realtime_effort_publisher_ = - std::make_shared>( - effort_publisher_); -#if !REALTIME_TOOLS_NEW_API - realtime_effort_publisher_->msg_.data.resize(params_.joints.size(), 0.0); -#else - effort_msg_.data.resize(params_.joints.size(), 0.0); -#endif - publish_interval_ = - rclcpp::Duration::from_seconds(1.0 / params_.effort_publisher.publish_frequency); - RCLCPP_INFO( - get_node()->get_logger(), - "Commanded effort topic: %s at %.3f Hz", - params_.effort_publisher.topic.c_str(), - params_.effort_publisher.publish_frequency); - } else { - RCLCPP_INFO(get_node()->get_logger(), "Commanded effort publisher is disabled."); - } - // Initialize target state vectors target_position_ = Eigen::Vector3d::Zero(); target_orientation_ = Eigen::Quaterniond::Identity(); @@ -658,8 +602,6 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat desired_position_ = target_position_; desired_orientation_ = target_orientation_; - publish_elapsed_ = rclcpp::Duration(0, 0); - RCLCPP_INFO(get_node()->get_logger(), "Controller activated."); return CallbackReturn::SUCCESS; } diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml index 24fdb00..a19f68d 100644 --- a/src/cartesian_controller.yaml +++ b/src/cartesian_controller.yaml @@ -181,7 +181,7 @@ cartesian_controller: use_coriolis_compensation: type: bool - default_value: true + default_value: false description: "Use coriolis compensation" use_gravity_compensation: @@ -299,16 +299,6 @@ cartesian_controller: default_value: False description: "If set to true, we will stop sending commands to the robot. This is mainly for debugging useful." - effort_publisher: - topic: - type: string - default_value: "commanded_effort" - description: "Topic name for publishing commanded joint efforts as std_msgs/Float64MultiArray. Values are in joints order." - publish_frequency: - type: double - default_value: 1000.0 - description: "Frequency in Hz for publishing commanded joint efforts. Values <= 0.0 disable publishing." - friction: fp1: type: double_array From 4c7c3f5e8ba6e6d9fe7221f4e88e0298e44b655b Mon Sep 17 00:00:00 2001 From: David Alexander Date: Wed, 24 Jun 2026 23:43:46 +0000 Subject: [PATCH 3/3] chore: maintain backwards compatibility and update docs --- crisp_controllers.xml | 12 +++++++ docs/new_robot_setup.md | 33 ++++++++++++++++--- .../crisp_controllers/state_broadcaster.hpp | 3 ++ src/state_broadcaster.cpp | 23 +++++++++++++ src/state_broadcaster.yaml | 8 +++++ 5 files changed, 74 insertions(+), 5 deletions(-) diff --git a/crisp_controllers.xml b/crisp_controllers.xml index 50aad2c..ed34c69 100644 --- a/crisp_controllers.xml +++ b/crisp_controllers.xml @@ -17,6 +17,18 @@ Broadcaster for joint, Cartesian pose, twist, and wrench state outputs. + + + Backwards-compatible alias for StateBroadcaster. + + + + + Backwards-compatible alias for StateBroadcaster. + + diff --git a/docs/new_robot_setup.md b/docs/new_robot_setup.md index 79121e2..9aeca7a 100644 --- a/docs/new_robot_setup.md +++ b/docs/new_robot_setup.md @@ -47,8 +47,8 @@ For more information on the controllers, check the [controllers details](getting ros__parameters: update_rate: 1000 # Hz - pose_broadcaster: - type: crisp_controllers/PoseBroadcaster + state_broadcaster: + type: crisp_controllers/StateBroadcaster gravity_compensation: type: crisp_controllers/CartesianController @@ -61,7 +61,7 @@ For more information on the controllers, check the [controllers details](getting # more controllers... /**: - pose_broadcaster: + state_broadcaster: ros__parameters: joints: - @@ -69,6 +69,14 @@ For more information on the controllers, check the [controllers details](getting end_effector_frame: base_frame: base + pose: + topic: current_pose + publish_frequency: 500.0 + + twist: + topic: current_twist + publish_frequency: 500.0 + gravity_compensation: ros__parameters: joints: @@ -182,12 +190,28 @@ For more information on the controllers, check the [controllers details](getting Node( package="controller_manager", executable="spawner", - arguments=["pose_broadcaster"], + arguments=["state_broadcaster"], output="screen", ), ... ``` + !!! note "Migrating old pose/twist broadcaster parameters" + `crisp_controllers/StateBroadcaster` replaces the old pose and twist broadcaster setup. If an old config used flat `topic` or `publish_frequency` parameters, move them under the matching output: + + ```yaml + state_broadcaster: + ros__parameters: + pose: + topic: current_pose + publish_frequency: 500.0 + twist: + topic: current_twist + publish_frequency: 500.0 + ``` + + Old pose broadcaster `topic` and `publish_frequency` values become `pose.topic` and `pose.publish_frequency`. Old twist broadcaster values become `twist.topic` and `twist.publish_frequency`. If both old broadcasters existed, merge both nested blocks into the single `state_broadcaster` config. + 6. After launching your robot you should see that new controller are being loaded. If you get stuck somewhere in the process feel free to open an issue. 7. Finally, to use the robots in crisp_py, add a configuration file for the new robot and Gymnasium environments that use it. @@ -234,4 +258,3 @@ For more information on the controllers, check the [controllers details](getting In a similar manner, you can add this config to an Gymnasium environment to create a Gymnasium env with this config! 8. Voila, you are good to go! - diff --git a/include/crisp_controllers/state_broadcaster.hpp b/include/crisp_controllers/state_broadcaster.hpp index 5fc67da..a0b2f8f 100644 --- a/include/crisp_controllers/state_broadcaster.hpp +++ b/include/crisp_controllers/state_broadcaster.hpp @@ -282,6 +282,9 @@ class StateBroadcaster : public controller_interface::ControllerInterface { bool configure_publish_interval( const std::string & output_name, double publish_frequency, PublishTimer & timer) const; + /** @brief Warn when deprecated parameters are supplied. */ + void warn_legacy_parameters() const; + /** * @brief Validate common parameters required before model and publisher setup. * @return True when required joints, frames, and topics are configured. diff --git a/src/state_broadcaster.cpp b/src/state_broadcaster.cpp index ca2cd2d..578744f 100644 --- a/src/state_broadcaster.cpp +++ b/src/state_broadcaster.cpp @@ -240,6 +240,8 @@ CallbackReturn StateBroadcaster::on_init() { CallbackReturn StateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + warn_legacy_parameters(); + if (!validate_common_parameters() || !build_model() || !cache_joint_model_indices()) { return CallbackReturn::ERROR; } @@ -413,6 +415,27 @@ bool StateBroadcaster::configure_publish_interval( return true; } +void StateBroadcaster::warn_legacy_parameters() const { + const auto & overrides = get_node()->get_node_parameters_interface()->get_parameter_overrides(); + const bool has_legacy_topic = overrides.find("topic") != overrides.end(); + const bool has_legacy_publish_frequency = overrides.find("publish_frequency") != overrides.end(); + + if (!has_legacy_topic && !has_legacy_publish_frequency) { + return; + } + + const char * legacy_params = has_legacy_topic && has_legacy_publish_frequency + ? "'topic' and 'publish_frequency' were" + : has_legacy_topic ? "'topic' was" : "'publish_frequency' was"; + + RCLCPP_WARN( + get_node()->get_logger(), + "Deprecated flat StateBroadcaster parameter%s %s supplied and ignored. " + "Use pose.topic/pose.publish_frequency or twist.topic/twist.publish_frequency instead. " + "Configured nested values, or their defaults, will be used.", + has_legacy_topic && has_legacy_publish_frequency ? "s" : "", legacy_params); +} + bool StateBroadcaster::validate_common_parameters() const { if (params_.joints.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure because joints is empty."); diff --git a/src/state_broadcaster.yaml b/src/state_broadcaster.yaml index eef5822..6d846ba 100644 --- a/src/state_broadcaster.yaml +++ b/src/state_broadcaster.yaml @@ -27,6 +27,14 @@ state_broadcaster: description: "EMA smoothing factor for estimated joint acceleration; higher values smooth more" validation: bounds<>: [0.0, 1.0] + topic: + type: string + default_value: "" + description: "Deprecated legacy flat topic parameter. Ignored; use pose.topic or twist.topic instead." + publish_frequency: + type: double + default_value: 0.0 + description: "Deprecated legacy flat publish frequency parameter. Ignored; use pose.publish_frequency or twist.publish_frequency instead." pose: topic: type: string