From 53eee4b07110edae313f6e2c0a42a1ed6f30d27e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20San=20Jos=C3=A9=20Pro?= <42489409+danielsanjosepro@users.noreply.github.com> Date: Fri, 22 Aug 2025 09:33:30 +0200 Subject: [PATCH 1/6] feat: broadcast frequency --- include/crisp_controllers/pose_broadcaster.hpp | 1 + src/pose_broadcaster.cpp | 13 ++++++++++++- src/pose_broadcaster.yaml | 4 ++++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/include/crisp_controllers/pose_broadcaster.hpp b/include/crisp_controllers/pose_broadcaster.hpp index 11587cc..324541d 100644 --- a/include/crisp_controllers/pose_broadcaster.hpp +++ b/include/crisp_controllers/pose_broadcaster.hpp @@ -64,6 +64,7 @@ class PoseBroadcaster {"JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; Eigen::VectorXd q; + rclcpp::Time last_publish_time_; }; } // namespace crisp_controllers diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp index f0763cb..16ebc90 100644 --- a/src/pose_broadcaster.cpp +++ b/src/pose_broadcaster.cpp @@ -62,7 +62,15 @@ PoseBroadcaster::update(const rclcpp::Time &time, auto current_quaternion = Eigen::Quaterniond(current_pose.rotation()); - if (realtime_pose_publisher_ && realtime_pose_publisher_->trylock()) + // Decide whether to publish the pose or not + bool should_publish = true; + if (params_.publish_frequency > 0.0) { + auto time_since_last = time - last_publish_time_; + auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); + should_publish = time_since_last >= min_interval; + } + + if (should_publish && realtime_pose_publisher_ && realtime_pose_publisher_->trylock()) { auto & pose_msg = realtime_pose_publisher_->msg_; @@ -76,6 +84,7 @@ PoseBroadcaster::update(const rclcpp::Time &time, pose_msg.pose.orientation.z = current_quaternion.z(); pose_msg.pose.orientation.w = current_quaternion.w(); realtime_pose_publisher_->unlockAndPublish(); + last_publish_time_ = time; } return controller_interface::return_type::OK; @@ -158,6 +167,8 @@ CallbackReturn PoseBroadcaster::on_configure( realtime_pose_publisher_ = std::make_shared>( pose_publisher_); + + last_publish_time_ = rclcpp::Time(0); return CallbackReturn::SUCCESS; } diff --git a/src/pose_broadcaster.yaml b/src/pose_broadcaster.yaml index 7d882f0..3c687d6 100644 --- a/src/pose_broadcaster.yaml +++ b/src/pose_broadcaster.yaml @@ -9,3 +9,7 @@ pose_broadcaster: 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 45172738e656ae0c1132e97a99fffe1207741747 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20San=20Jos=C3=A9=20Pro?= <42489409+danielsanjosepro@users.noreply.github.com> Date: Mon, 8 Sep 2025 10:31:56 +0200 Subject: [PATCH 2/6] First draft of twist broadcaster --- CMakeLists.txt | 7 + crisp_controllers.xml | 6 + .../crisp_controllers/twist_broadcaster.hpp | 71 +++++++ src/twist_broadcaster.cpp | 193 ++++++++++++++++++ src/twist_broadcaster.yaml | 15 ++ 5 files changed, 292 insertions(+) create mode 100644 include/crisp_controllers/twist_broadcaster.hpp create mode 100644 src/twist_broadcaster.cpp create mode 100644 src/twist_broadcaster.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 8eca517..2176f74 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,12 +51,18 @@ generate_parameter_library( src/torque_feedback_controller.yaml ) +generate_parameter_library( + twist_broadcaster_parameters + src/twist_broadcaster.yaml +) + add_library( ${PROJECT_NAME} SHARED src/cartesian_controller.cpp src/pose_broadcaster.cpp src/torque_feedback_controller.cpp + src/twist_broadcaster.cpp ) target_include_directories(${PROJECT_NAME} @@ -78,6 +84,7 @@ target_link_libraries(${PROJECT_NAME} cartesian_impedance_controller_parameters pose_broadcaster_parameters torque_feedback_controller_parameters + twist_broadcaster_parameters ) diff --git a/crisp_controllers.xml b/crisp_controllers.xml index fd16063..0b0d64e 100644 --- a/crisp_controllers.xml +++ b/crisp_controllers.xml @@ -17,4 +17,10 @@ Torque feedback controller for robotic manipulators that responds to external torques with PD control and friction compensation. + + + Simple broadcaster for the twist velocity. + + diff --git a/include/crisp_controllers/twist_broadcaster.hpp b/include/crisp_controllers/twist_broadcaster.hpp new file mode 100644 index 0000000..02ae7f6 --- /dev/null +++ b/include/crisp_controllers/twist_broadcaster.hpp @@ -0,0 +1,71 @@ +#pragma once +#include + +#include +#include +#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::Time last_publish_time_; +}; + +} // namespace crisp_controllers \ No newline at end of file diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp new file mode 100644 index 0000000..5c1c400 --- /dev/null +++ b/src/twist_broadcaster.cpp @@ -0,0 +1,193 @@ +#include +#include + +#include +#include +#include +#include +#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]; + + q[i] = state_interfaces_[i*2].get_value(); + q_dot[i] = state_interfaces_[i*2+1].get_value(); + + 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 + bool should_publish = true; + if (params_.publish_frequency > 0.0) { + auto time_since_last = time - last_publish_time_; + auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); + should_publish = time_since_last >= min_interval; + } + + if (should_publish && realtime_twist_publisher_ && realtime_twist_publisher_->trylock()) + { + auto & twist_msg = realtime_twist_publisher_->msg_; + + twist_msg.header.stamp = time; + twist_msg.header.frame_id = params_.base_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]; + realtime_twist_publisher_->unlockAndPublish(); + last_publish_time_ = time; + } + + 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 (not 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() and 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 (not 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; + } + } + + 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_); + + last_publish_time_ = rclcpp::Time(0); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TwistBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { + 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 new file mode 100644 index 0000000..2e07a8e --- /dev/null +++ b/src/twist_broadcaster.yaml @@ -0,0 +1,15 @@ +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)" \ No newline at end of file From 6ac4eeaf15532061b9b74bf13472274b837a6246 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20San=20Jos=C3=A9=20Pro?= <42489409+danielsanjosepro@users.noreply.github.com> Date: Mon, 8 Sep 2025 11:22:24 +0200 Subject: [PATCH 3/6] Fix import + formatting --- .../crisp_controllers/twist_broadcaster.hpp | 2 +- src/twist_broadcaster.cpp | 264 +++++++++--------- src/twist_broadcaster.yaml | 2 +- 3 files changed, 134 insertions(+), 134 deletions(-) diff --git a/include/crisp_controllers/twist_broadcaster.hpp b/include/crisp_controllers/twist_broadcaster.hpp index 02ae7f6..8838a6c 100644 --- a/include/crisp_controllers/twist_broadcaster.hpp +++ b/include/crisp_controllers/twist_broadcaster.hpp @@ -68,4 +68,4 @@ class TwistBroadcaster rclcpp::Time last_publish_time_; }; -} // namespace crisp_controllers \ No newline at end of file +} // namespace crisp_controllers diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp index 5c1c400..e9a1fb4 100644 --- a/src/twist_broadcaster.cpp +++ b/src/twist_broadcaster.cpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include #include #include @@ -18,172 +18,172 @@ 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 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::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*/) { + 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); + 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++) { + 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]; + auto joint_name = params_.joints[i]; + auto joint_id = model_.getJointId(joint_name); + auto joint = model_.joints[joint_id]; - q[i] = state_interfaces_[i*2].get_value(); - q_dot[i] = state_interfaces_[i*2+1].get_value(); - - 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]; + q[i] = state_interfaces_[i*2].get_value(); + q_dot[i] = state_interfaces_[i*2+1].get_value(); + + 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 - bool should_publish = true; - if (params_.publish_frequency > 0.0) { - auto time_since_last = time - last_publish_time_; - auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); - should_publish = time_since_last >= min_interval; - } - - if (should_publish && realtime_twist_publisher_ && realtime_twist_publisher_->trylock()) - { - auto & twist_msg = realtime_twist_publisher_->msg_; - - twist_msg.header.stamp = time; - twist_msg.header.frame_id = params_.base_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]; - realtime_twist_publisher_->unlockAndPublish(); - last_publish_time_ = time; - } - - return controller_interface::return_type::OK; + + 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 + bool should_publish = true; + if (params_.publish_frequency > 0.0) { + auto time_since_last = time - last_publish_time_; + auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); + should_publish = time_since_last >= min_interval; + } + + if (should_publish && realtime_twist_publisher_ && realtime_twist_publisher_->trylock()) + { + auto & twist_msg = realtime_twist_publisher_->msg_; + + twist_msg.header.stamp = time; + twist_msg.header.frame_id = params_.base_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]; + realtime_twist_publisher_->unlockAndPublish(); + last_publish_time_ = time; + } + + 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(); + // Initialize parameters + params_listener_ = + std::make_shared(get_node()); + params_listener_->refresh_dynamic_parameters(); + params_ = params_listener_->get_params(); - return CallbackReturn::SUCCESS; + 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 (not 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; + 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; } - } - 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() and 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)); + + 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()); } - } - 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_); + // First we check that the passed joints exist in the kineatic tree + for (auto& joint : params_.joints) { + if (not 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() and 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)); + } + } - for (int joint_id = 0; joint_id < model_.njoints; joint_id++) { - if (model_.names[joint_id] == "universe") { - continue; + 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 (not 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 (not 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; - } - } - - 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_); - - last_publish_time_ = rclcpp::Time(0); - return CallbackReturn::SUCCESS; + + 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_); + + last_publish_time_ = rclcpp::Time(0); + return CallbackReturn::SUCCESS; } CallbackReturn TwistBroadcaster::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - return CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn TwistBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - return CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } } // namespace crisp_controllers diff --git a/src/twist_broadcaster.yaml b/src/twist_broadcaster.yaml index 2e07a8e..a455667 100644 --- a/src/twist_broadcaster.yaml +++ b/src/twist_broadcaster.yaml @@ -12,4 +12,4 @@ twist_broadcaster: publish_frequency: type: double default_value: 250.0 - description: "Publishing frequency in Hz (0.0 means no throttling)" \ No newline at end of file + description: "Publishing frequency in Hz (0.0 means no throttling)" From 92b62ae728649c975b3d0c57d27fa4fbbe1f331c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20San=20Jos=C3=A9=20Pro?= <42489409+danielsanjosepro@users.noreply.github.com> Date: Mon, 8 Sep 2025 12:15:57 +0200 Subject: [PATCH 4/6] Fixing a time issue in ROS --- src/pose_broadcaster.cpp | 2 +- src/twist_broadcaster.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp index 16ebc90..7de3bfb 100644 --- a/src/pose_broadcaster.cpp +++ b/src/pose_broadcaster.cpp @@ -168,7 +168,7 @@ CallbackReturn PoseBroadcaster::on_configure( std::make_shared>( pose_publisher_); - last_publish_time_ = rclcpp::Time(0); + last_publish_time_ = this->get_node()->now(); return CallbackReturn::SUCCESS; } diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp index e9a1fb4..1724db3 100644 --- a/src/twist_broadcaster.cpp +++ b/src/twist_broadcaster.cpp @@ -79,7 +79,7 @@ TwistBroadcaster::update(const rclcpp::Time &time, auto & twist_msg = realtime_twist_publisher_->msg_; twist_msg.header.stamp = time; - twist_msg.header.frame_id = params_.base_frame; + 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]; @@ -172,7 +172,7 @@ CallbackReturn TwistBroadcaster::on_configure( std::make_shared>( twist_publisher_); - last_publish_time_ = rclcpp::Time(0); + last_publish_time_ = this->get_node()->now(); return CallbackReturn::SUCCESS; } From 2806ca7eec03c48e1ef3ba305c57f9cde4b5dc5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20San=20Jos=C3=A9=20Pro?= <42489409+danielsanjosepro@users.noreply.github.com> Date: Mon, 8 Sep 2025 12:17:08 +0200 Subject: [PATCH 5/6] feat: add twist_broadcaster that publishes end effector local twist From bbaae798c6f56804ef41e87711597ae1529f62f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20San=20Jos=C3=A9=20Pro?= <42489409+danielsanjosepro@users.noreply.github.com> Date: Mon, 8 Sep 2025 12:17:26 +0200 Subject: [PATCH 6/6] feat: add throttled pose / twist broadcasting