From fee02e703e2dae595bad215466b7401517da8b66 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Jul 2024 17:18:55 +0200 Subject: [PATCH 1/5] added force and torque offsets to the parameters of the force_torque_sensor_broadcaster --- .../force_torque_sensor_broadcaster.hpp | 2 ++ .../src/force_torque_sensor_broadcaster.cpp | 15 +++++++++ ..._torque_sensor_broadcaster_parameters.yaml | 33 +++++++++++++++++++ .../test_force_torque_sensor_broadcaster.cpp | 31 +++++++++++++++++ 4 files changed, 81 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 bd477ed68a..fabcd03fc7 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 @@ -63,6 +63,8 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: + void apply_sensor_offset(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 9b570d353f..a3a3d3abf8 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -144,16 +144,31 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate controller_interface::return_type ForceTorqueSensorBroadcaster::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + } if (realtime_publisher_ && realtime_publisher_->trylock()) { 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_); realtime_publisher_->unlockAndPublish(); } return controller_interface::return_type::OK; } +void ForceTorqueSensorBroadcaster::apply_sensor_offset( + const Params & params, geometry_msgs::msg::WrenchStamped & msg) +{ + msg.wrench.force.x -= params.offset.force.x; + msg.wrench.force.y -= params.offset.force.y; + msg.wrench.force.z -= params.offset.force.z; + msg.wrench.torque.x -= params.offset.torque.x; + msg.wrench.torque.y -= params.offset.torque.y; + msg.wrench.torque.z -= params.offset.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 3e75ab6012..0869f5cf3c 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 @@ -46,3 +46,36 @@ force_torque_sensor_broadcaster: default_value: "", description: "Name of the state interface with torque values around 'z' axis.", } + offset: + force: + x: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'z' axis.", + } + torque: + x: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'z' axis.", + } 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 1ea25520cc..f155025613 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 @@ -278,6 +278,37 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets) +{ + SetUpFTSBroadcaster(); + + std::array force_offsets = {10.0, 30.0, -50.0}; + std::array torque_offsets = {1.0, -1.2, -5.2}; + // set the params 'sensor_name' and 'frame_id' + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.x", force_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.y", force_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.z", force_offsets[2]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.x", torque_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.y", torque_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.z", torque_offsets[2]}); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_msg; + subscribe_and_get_message(wrench_msg); + + ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] - force_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] - force_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] - force_offsets[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] - torque_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] - torque_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] - torque_offsets[2]); +} + TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { SetUpFTSBroadcaster(); From b287c540759acac6b31823b10d6d22d0ef787033 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 14:53:05 +0200 Subject: [PATCH 2/5] Change the ForceTorqueSensorBroadcaster to a ChainableController to expose interfaces removing the offsets --- .../force_torque_sensor_broadcaster.xml | 2 +- .../force_torque_sensor_broadcaster.hpp | 14 +++- .../src/force_torque_sensor_broadcaster.cpp | 65 ++++++++++++++++++- 3 files changed, 74 insertions(+), 7 deletions(-) diff --git a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml index 10d19a93c5..8e61e013de 100644 --- a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml +++ b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml @@ -1,6 +1,6 @@ + type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface"> This controller publishes the readings of force-torque interfaces as geometry_msgs/WrenchStamped message. 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 fabcd03fc7..e5a5349c32 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 @@ -20,8 +20,9 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include +#include -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" @@ -32,7 +33,7 @@ namespace force_torque_sensor_broadcaster { -class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface +class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface { public: FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC @@ -59,9 +60,16 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update( + controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + std::vector on_export_state_interfaces() override; + protected: void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg); 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 a3a3d3abf8..51cb94513b 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -24,7 +24,7 @@ namespace force_torque_sensor_broadcaster { ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() -: controller_interface::ControllerInterface() +: controller_interface::ChainableControllerInterface() { } @@ -141,7 +141,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type ForceTorqueSensorBroadcaster::update( +controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { if (param_listener_->is_old(params_)) @@ -159,6 +159,65 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update( return controller_interface::return_type::OK; } +controller_interface::return_type ForceTorqueSensorBroadcaster::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +std::vector +ForceTorqueSensorBroadcaster::on_export_state_interfaces() +{ + std::vector exported_state_interfaces; + + std::vector force_names( + {params_.interface_names.force.x, params_.interface_names.force.y, + params_.interface_names.force.z}); + std::vector torque_names( + {params_.interface_names.torque.x, params_.interface_names.torque.y, + params_.interface_names.torque.z}); + if (!params_.sensor_name.empty()) + { + const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names(); + std::copy( + semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin()); + std::copy( + semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin()); + } + const std::string controller_name = get_node()->get_name(); + if (!force_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); + } + if (!force_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); + } + if (!force_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); + } + if (!torque_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); + } + if (!torque_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); + } + if (!torque_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); + } + return exported_state_interfaces; +} + void ForceTorqueSensorBroadcaster::apply_sensor_offset( const Params & params, geometry_msgs::msg::WrenchStamped & msg) { @@ -175,4 +234,4 @@ void ForceTorqueSensorBroadcaster::apply_sensor_offset( PLUGINLIB_EXPORT_CLASS( force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) From b2924e238f25485adad10122dd234872b2c779de Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 14:54:24 +0200 Subject: [PATCH 3/5] Added tests for the exported state interfaces --- .../test_force_torque_sensor_broadcaster.cpp | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) 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 f155025613..a4ddf68786 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 @@ -307,6 +307,29 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] - torque_offsets[0]); ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] - torque_offsets[1]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] - torque_offsets[2]); + + // Check the exported state interfaces + 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"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ( + exported_state_interfaces[i].get_value(), + sensor_values_[i] - (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); + } } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) @@ -331,6 +354,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.x)); ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.y)); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 2u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0].get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1].get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0].get_value(), sensor_values_[0]); + ASSERT_EQ(exported_state_interfaces[1].get_value(), sensor_values_[5]); } TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) @@ -359,6 +391,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]); ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + 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 + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1].get_name(), controller_name + "/fts_sensor/force.y"); + ASSERT_EQ(exported_state_interfaces[2].get_name(), controller_name + "/fts_sensor/force.z"); + ASSERT_EQ(exported_state_interfaces[3].get_name(), controller_name + "/fts_sensor/torque.x"); + ASSERT_EQ(exported_state_interfaces[4].get_name(), controller_name + "/fts_sensor/torque.y"); + ASSERT_EQ(exported_state_interfaces[5].get_name(), controller_name + "/fts_sensor/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ(exported_state_interfaces[i].get_value(), sensor_values_[i]); + } } int main(int argc, char ** argv) From f7643ba2734df211dcdcd83922e307626743216b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 15 Aug 2024 14:20:04 +0200 Subject: [PATCH 4/5] add offsets instead of substracting them --- .../src/force_torque_sensor_broadcaster.cpp | 12 ++++++------ .../test/test_force_torque_sensor_broadcaster.cpp | 14 +++++++------- 2 files changed, 13 insertions(+), 13 deletions(-) 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 51cb94513b..ae105a511c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -221,12 +221,12 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces() void ForceTorqueSensorBroadcaster::apply_sensor_offset( const Params & params, geometry_msgs::msg::WrenchStamped & msg) { - msg.wrench.force.x -= params.offset.force.x; - msg.wrench.force.y -= params.offset.force.y; - msg.wrench.force.z -= params.offset.force.z; - msg.wrench.torque.x -= params.offset.torque.x; - msg.wrench.torque.y -= params.offset.torque.y; - msg.wrench.torque.z -= params.offset.torque.z; + msg.wrench.force.x += params.offset.force.x; + msg.wrench.force.y += params.offset.force.y; + msg.wrench.force.z += params.offset.force.z; + msg.wrench.torque.x += params.offset.torque.x; + msg.wrench.torque.y += params.offset.torque.y; + msg.wrench.torque.z += params.offset.torque.z; } } // namespace force_torque_sensor_broadcaster 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 a4ddf68786..4540821630 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 @@ -301,12 +301,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets subscribe_and_get_message(wrench_msg); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); - ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] - force_offsets[0]); - ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] - force_offsets[1]); - ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] - force_offsets[2]); - ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] - torque_offsets[0]); - ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] - torque_offsets[1]); - ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] - torque_offsets[2]); + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] + force_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] + force_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] + force_offsets[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] + torque_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] + torque_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] + torque_offsets[2]); // Check the exported state interfaces const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); @@ -328,7 +328,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets { ASSERT_EQ( exported_state_interfaces[i].get_value(), - sensor_values_[i] - (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); + sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); } } From c1658c3e13e803de17c4bc80a9185d2903b39508 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 8 Oct 2024 17:02:20 +0200 Subject: [PATCH 5/5] add compilation fixes --- .../test_force_torque_sensor_broadcaster.cpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) 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 4540821630..e436beb2e5 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 @@ -313,21 +313,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets 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"); + 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"); + 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"); + 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"); + 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"); + 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"); + exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); for (size_t i = 0; i < 6; ++i) { ASSERT_EQ( - exported_state_interfaces[i].get_value(), + exported_state_interfaces[i]->get_value(), sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); } } @@ -359,10 +359,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); ASSERT_EQ(exported_state_interfaces.size(), 2u); const std::string controller_name = fts_broadcaster_->get_node()->get_name(); - ASSERT_EQ(exported_state_interfaces[0].get_name(), controller_name + "/fts_sensor/force.x"); - ASSERT_EQ(exported_state_interfaces[1].get_name(), controller_name + "/fts_sensor/torque.z"); - ASSERT_EQ(exported_state_interfaces[0].get_value(), sensor_values_[0]); - ASSERT_EQ(exported_state_interfaces[1].get_value(), sensor_values_[5]); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); + ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); } TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) @@ -396,15 +396,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) 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 + "/fts_sensor/force.x"); - ASSERT_EQ(exported_state_interfaces[1].get_name(), controller_name + "/fts_sensor/force.y"); - ASSERT_EQ(exported_state_interfaces[2].get_name(), controller_name + "/fts_sensor/force.z"); - ASSERT_EQ(exported_state_interfaces[3].get_name(), controller_name + "/fts_sensor/torque.x"); - ASSERT_EQ(exported_state_interfaces[4].get_name(), controller_name + "/fts_sensor/torque.y"); - ASSERT_EQ(exported_state_interfaces[5].get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_name(), controller_name + "/fts_sensor/force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z"); for (size_t i = 0; i < 6; ++i) { - ASSERT_EQ(exported_state_interfaces[i].get_value(), sensor_values_[i]); + ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); } }