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)"