Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces #1215

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<library path="force_torque_sensor_broadcaster">
<class name="force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"
type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ControllerInterface">
type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface">
<description>
This controller publishes the readings of force-torque interfaces as geometry_msgs/WrenchStamped message.
</description>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,9 @@
#define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_

#include <memory>
#include <vector>

#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"
Expand All @@ -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
Expand All @@ -59,10 +60,19 @@ 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<hardware_interface::StateInterface> on_export_state_interfaces() override;

protected:
void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg);

std::shared_ptr<ParamListener> param_listener_;
Params params_;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
namespace force_torque_sensor_broadcaster
{
ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster()
: controller_interface::ControllerInterface()
: controller_interface::ChainableControllerInterface()
{
}

Expand Down Expand Up @@ -141,23 +141,97 @@ 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_))
{
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;
}

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<hardware_interface::StateInterface>
ForceTorqueSensorBroadcaster::on_export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> exported_state_interfaces;

std::vector<std::string> force_names(
{params_.interface_names.force.x, params_.interface_names.force.y,
params_.interface_names.force.z});
std::vector<std::string> 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)
{
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"

PLUGINLIB_EXPORT_CLASS(
force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster,
controller_interface::ControllerInterface)
controller_interface::ChainableControllerInterface)
Original file line number Diff line number Diff line change
Expand Up @@ -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.",
}
Original file line number Diff line number Diff line change
Expand Up @@ -278,6 +278,60 @@ 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<double, 3> force_offsets = {10.0, 30.0, -50.0};
std::array<double, 3> 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]);

// 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)
{
SetUpFTSBroadcaster();
Expand All @@ -300,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)
Expand Down Expand Up @@ -328,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)
Expand Down
Loading