From 914ab5b6236740c3ceb80bbb8006d88152e6df42 Mon Sep 17 00:00:00 2001 From: Edward Date: Fri, 18 Apr 2025 18:37:27 -0500 Subject: [PATCH] [force_torque_sensor_broadcaster] Add multiplier feature Signed-off-by: Edward --- .../force_torque_sensor_broadcaster.hpp | 1 + .../src/force_torque_sensor_broadcaster.cpp | 11 +++ ..._torque_sensor_broadcaster_parameters.yaml | 33 +++++++++ .../test_force_torque_sensor_broadcaster.cpp | 69 +++++++++++++++++++ 4 files changed, 114 insertions(+) diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 34b7f242ee..0c8ce438f1 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -61,6 +61,7 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr protected: void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg); + void apply_sensor_multiplier(const Params & params, geometry_msgs::msg::WrenchStamped & msg); std::shared_ptr param_listener_; Params params_; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index ef5e74e89b..c66332182d 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -175,6 +175,7 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write realtime_publisher_->msg_.header.stamp = time; force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); this->apply_sensor_offset(params_, realtime_publisher_->msg_); + this->apply_sensor_multiplier(params_, realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); } @@ -267,6 +268,16 @@ void ForceTorqueSensorBroadcaster::apply_sensor_offset( msg.wrench.torque.y += params.offset.torque.y; msg.wrench.torque.z += params.offset.torque.z; } + +void ForceTorqueSensorBroadcaster::apply_sensor_multiplier( + const Params & params, geometry_msgs::msg::WrenchStamped & msg) { + msg.wrench.force.x *= params.multiplier.force.x; + msg.wrench.force.y *= params.multiplier.force.y; + msg.wrench.force.z *= params.multiplier.force.z; + msg.wrench.torque.x *= params.multiplier.torque.x; + msg.wrench.torque.y *= params.multiplier.torque.y; + msg.wrench.torque.z *= params.multiplier.torque.z; +} } // namespace force_torque_sensor_broadcaster #include "pluginlib/class_list_macros.hpp" diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 0869f5cf3c..7970c0920e 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -79,3 +79,36 @@ force_torque_sensor_broadcaster: default_value: 0.0, description: "The offset of torque values around 'z' axis.", } + multiplier: + force: + x: { + type: double, + default_value: 1.0, + description: "The multiplier of force value on 'x' axis.", + } + y: { + type: double, + default_value: 1.0, + description: "The multiplier of force value on 'y' axis.", + } + z: { + type: double, + default_value: 1.0, + description: "The multiplier of force value on 'z' axis.", + } + torque: + x: { + type: double, + default_value: 1.0, + description: "The multiplier of torque value around 'x' axis.", + } + y: { + type: double, + default_value: 1.0, + description: "The multiplier of torque value around 'y' axis.", + } + z: { + type: double, + default_value: 1.0, + description: "The multiplier of torque value around 'z' axis.", + } \ No newline at end of file diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index f6781aab04..90ef6adbf8 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -355,6 +355,75 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets } } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipliers) +{ + SetUpFTSBroadcaster(); + + // some non‐trivial multipliers + std::array force_multipliers = {{2.0, 0.5, -1.0}}; + std::array torque_multipliers = {{-2.0, 3.0, 0.0}}; + + // Set the required params + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set all multiplier parameters + fts_broadcaster_->get_node()->set_parameter({"multiplier.force.x", force_multipliers[0]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.force.y", force_multipliers[1]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.force.z", force_multipliers[2]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.x", torque_multipliers[0]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.y", torque_multipliers[1]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.z", torque_multipliers[2]}); + + // Configure & activate + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Publish & grab the message + geometry_msgs::msg::WrenchStamped wrench_msg; + subscribe_and_get_message(wrench_msg); + + // Check header + ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); + + // Check that each field was scaled accordingly + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] * force_multipliers[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] * force_multipliers[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] * force_multipliers[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] * torque_multipliers[0]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] * torque_multipliers[1]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] * torque_multipliers[2]); + + // the exported state interfaces reflect the same scaled values + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ( + exported_state_interfaces[0]->get_name(), controller_name + "/" + sensor_name_ + "/force.x"); + ASSERT_EQ( + exported_state_interfaces[1]->get_name(), controller_name + "/" + sensor_name_ + "/force.y"); + ASSERT_EQ( + exported_state_interfaces[2]->get_name(), controller_name + "/" + sensor_name_ + "/force.z"); + ASSERT_EQ( + exported_state_interfaces[3]->get_name(), controller_name + "/" + sensor_name_ + "/torque.x"); + ASSERT_EQ( + exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y"); + ASSERT_EQ( + exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_interface_name(), "torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ(exported_state_interfaces[i]->get_value(), + sensor_values_[i]*(i < 3 ? force_multipliers[i] : torque_multipliers[i - 3])); + } +} + TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { SetUpFTSBroadcaster();