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.

Sign up for GitHub

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

[FTS Broadcaster] Add support for publishing forces in other frames on additional topics #268

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
+133 −37
Open

[FTS Broadcaster] Add support for publishing forces in other frames on additional topics #268

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f893c57
Add publication of forces on additional frames.
destogl Nov 17, 2021
f09240c
Apply suggestions from code review
bmagyar Nov 19, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 13 additions & 3 deletions force_torque_sensor_broadcaster/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,9 @@ find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(realtime_tools REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)

add_library(
force_torque_sensor_broadcaster
Expand All @@ -32,13 +35,16 @@ target_include_directories(
)
ament_target_dependencies(
force_torque_sensor_broadcaster
geometry_msgs
controller_interface
geometry_msgs
hardware_interface
pluginlib
rclcpp
rclcpp_lifecycle
realtime_tools
tf2
tf2_geometry_msgs
tf2_ros
)
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
Expand Down Expand Up @@ -103,11 +109,15 @@ ament_export_libraries(
)
ament_export_dependencies(
controller_interface
geometry_msgs
hardware_interface
pluginlib
rclcpp
rclcpp_lifecycle
geometry_msgs
hardware_interface
realtime_tools
tf2
tf2_geometry_msgs
tf2_ros
)

ament_package()
21 changes: 18 additions & 3 deletions ...r_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,14 @@
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/realtime_publisher.h"
#include "semantic_components/force_torque_sensor.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

namespace force_torque_sensor_broadcaster
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
using WrenchMsgType = geometry_msgs::msg::WrenchStamped;

class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface
{
Expand Down Expand Up @@ -66,12 +70,23 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte
std::string sensor_name_;
std::array<std::string, 6> interface_names_;
std::string frame_id_;
std::vector<std::string> additional_frames_to_publish_;

std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_;

using StatePublisher = realtime_tools::RealtimePublisher<geometry_msgs::msg::WrenchStamped>;
rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr sensor_state_publisher_;
std::unique_ptr<StatePublisher> realtime_publisher_;
WrenchMsgType wrench_raw_;

using WrenchPublisher = rclcpp::Publisher<WrenchMsgType>::SharedPtr;
using WrenchRTPublisher = realtime_tools::RealtimePublisher<WrenchMsgType>;
WrenchPublisher wrench_raw_pub_;
std::unique_ptr<WrenchRTPublisher> wrench_raw_publisher_;

// Transformation variables
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

std::vector<WrenchPublisher> wrench_aditional_frames_pubs_;
std::vector<std::unique_ptr<WrenchRTPublisher>> wrench_aditional_frames_publishers_;
};

} // namespace force_torque_sensor_broadcaster
Expand Down
3 changes: 3 additions & 0 deletions force_torque_sensor_broadcaster/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>realtime_tools</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>

<test_depend>ament_cmake_gmock</test_depend>
<test_depend>controller_manager</test_depend>
Expand Down
129 changes: 98 additions & 31 deletions force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,13 @@

#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp"

#include <limits>
#include <memory>
#include <string>
bmagyar marked this conversation as resolved.
Show resolved Hide resolved

#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

namespace force_torque_sensor_broadcaster
{
ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster()
Expand All @@ -32,14 +36,16 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_init()
{
try
{
auto_declare<std::string>("sensor_name", "");
auto_declare<std::string>("interface_names.force.x", "");
auto_declare<std::string>("interface_names.force.y", "");
auto_declare<std::string>("interface_names.force.z", "");
auto_declare<std::string>("interface_names.torque.x", "");
auto_declare<std::string>("interface_names.torque.y", "");
auto_declare<std::string>("interface_names.torque.z", "");
auto_declare<std::string>("frame_id", "");
get_node()->declare_parameter<std::string>("sensor_name", "");
get_node()->declare_parameter<std::string>("interface_names.force.x", "");
get_node()->declare_parameter<std::string>("interface_names.force.y", "");
get_node()->declare_parameter<std::string>("interface_names.force.z", "");
get_node()->declare_parameter<std::string>("interface_names.torque.x", "");
get_node()->declare_parameter<std::string>("interface_names.torque.y", "");
get_node()->declare_parameter<std::string>("interface_names.torque.z", "");
get_node()->declare_parameter<std::string>("frame_id", "");
get_node()->declare_parameter<std::vector<std::string>>(
"additional_frames_to_publish", std::vector<std::string>({}));
}
catch (const std::exception & e)
{
Expand All @@ -53,21 +59,21 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_init()
CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
sensor_name_ = node_->get_parameter("sensor_name").as_string();
interface_names_[0] = node_->get_parameter("interface_names.force.x").as_string();
interface_names_[1] = node_->get_parameter("interface_names.force.y").as_string();
interface_names_[2] = node_->get_parameter("interface_names.force.z").as_string();
interface_names_[3] = node_->get_parameter("interface_names.torque.x").as_string();
interface_names_[4] = node_->get_parameter("interface_names.torque.y").as_string();
interface_names_[5] = node_->get_parameter("interface_names.torque.z").as_string();
sensor_name_ = get_node()->get_parameter("sensor_name").as_string();
interface_names_[0] = get_node()->get_parameter("interface_names.force.x").as_string();
interface_names_[1] = get_node()->get_parameter("interface_names.force.y").as_string();
interface_names_[2] = get_node()->get_parameter("interface_names.force.z").as_string();
interface_names_[3] = get_node()->get_parameter("interface_names.torque.x").as_string();
interface_names_[4] = get_node()->get_parameter("interface_names.torque.y").as_string();
interface_names_[5] = get_node()->get_parameter("interface_names.torque.z").as_string();

const bool no_interface_names_defined =
std::count(interface_names_.begin(), interface_names_.end(), "") == 6;

if (sensor_name_.empty() && no_interface_names_defined)
{
RCLCPP_ERROR(
node_->get_logger(),
get_node()->get_logger(),
"'sensor_name' or at least one "
"'interface_names.[force|torque].[x|y|z]' parameter has to be specified.");
return CallbackReturn::ERROR;
Expand All @@ -76,16 +82,16 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
if (!sensor_name_.empty() && !no_interface_names_defined)
{
RCLCPP_ERROR(
node_->get_logger(),
get_node()->get_logger(),
"both 'sensor_name' and "
"'interface_names.[force|torque].[x|y|z]' parameters can not be specified together.");
return CallbackReturn::ERROR;
}

frame_id_ = node_->get_parameter("frame_id").as_string();
frame_id_ = get_node()->get_parameter("frame_id").as_string();
if (frame_id_.empty())
{
RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided.");
RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided.");
return CallbackReturn::ERROR;
}

Expand All @@ -104,10 +110,10 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure(

try
{
// register ft sensor data publisher
sensor_state_publisher_ = node_->create_publisher<geometry_msgs::msg::WrenchStamped>(
"~/wrench", rclcpp::SystemDefaultsQoS());
realtime_publisher_ = std::make_unique<StatePublisher>(sensor_state_publisher_);
// register ft sensor data publishers
wrench_raw_pub_ =
get_node()->create_publisher<WrenchMsgType>("~/wrench", rclcpp::SystemDefaultsQoS());
wrench_raw_publisher_ = std::make_unique<WrenchRTPublisher>(wrench_raw_pub_);
}
catch (const std::exception & e)
{
Expand All @@ -117,11 +123,39 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
return CallbackReturn::ERROR;
}

realtime_publisher_->lock();
realtime_publisher_->msg_.header.frame_id = frame_id_;
realtime_publisher_->unlock();
wrench_raw_.header.frame_id = frame_id_;

wrench_raw_publisher_->lock();
wrench_raw_publisher_->msg_.header.frame_id = frame_id_;
wrench_raw_publisher_->unlock();

// Add additional frames to publish if any exits
additional_frames_to_publish_ =
get_node()->get_parameter("additional_frames_to_publish").as_string_array();

if (!additional_frames_to_publish_.empty())
{
auto nr_frames = additional_frames_to_publish_.size();
wrench_aditional_frames_pubs_.reserve(nr_frames);
wrench_aditional_frames_publishers_.reserve(nr_frames);
for (const auto & frame : additional_frames_to_publish_)
{
WrenchPublisher pub = get_node()->create_publisher<WrenchMsgType>(
"~/wrench_" + frame, rclcpp::SystemDefaultsQoS());
wrench_aditional_frames_pubs_.emplace_back(pub);
wrench_aditional_frames_publishers_.emplace_back(std::make_unique<WrenchRTPublisher>(pub));

wrench_aditional_frames_publishers_.back()->lock();
wrench_aditional_frames_publishers_.back()->msg_.header.frame_id = frame;
wrench_aditional_frames_publishers_.back()->unlock();
}

// initialize buffer transforms
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_node()->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
}

RCLCPP_DEBUG(node_->get_logger(), "configure successful");
RCLCPP_INFO(get_node()->get_logger(), "configure successful");
return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -159,11 +193,44 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate(
controller_interface::return_type ForceTorqueSensorBroadcaster::update(
const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
{
if (realtime_publisher_ && realtime_publisher_->trylock())
wrench_raw_.header.stamp = time;
force_torque_sensor_->get_values_as_message(wrench_raw_.wrench);

// Publish sensor data
if (wrench_raw_publisher_ && wrench_raw_publisher_->trylock())
{
wrench_raw_publisher_->msg_.header.stamp = time;
wrench_raw_publisher_->msg_.wrench = wrench_raw_.wrench;
wrench_raw_publisher_->unlockAndPublish();
}

for (const auto & publisher : wrench_aditional_frames_publishers_)
{
realtime_publisher_->msg_.header.stamp = time;
force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench);
realtime_publisher_->unlockAndPublish();
try
{
auto transform =
tf_buffer_->lookupTransform(publisher->msg_.header.frame_id, frame_id_, tf2::TimePointZero);
tf2::doTransform(wrench_raw_, publisher->msg_, transform);

if (publisher && publisher->trylock())
{
publisher->msg_.header.stamp = time;
publisher->unlockAndPublish();
}
}
catch (const tf2::TransformException & e)
{
RCLCPP_ERROR_SKIPFIRST_THROTTLE(
get_node()->get_logger(), *(get_node()->get_clock()), 5000,
"LookupTransform failed from '%s' to '%s'.", frame_id_.c_str(),
publisher->msg_.header.frame_id.c_str());
publisher->msg_.wrench.force.x = std::numeric_limits<double>::quiet_NaN();
publisher->msg_.wrench.force.y = std::numeric_limits<double>::quiet_NaN();
publisher->msg_.wrench.force.z = std::numeric_limits<double>::quiet_NaN();
publisher->msg_.wrench.torque.x = std::numeric_limits<double>::quiet_NaN();
publisher->msg_.wrench.torque.y = std::numeric_limits<double>::quiet_NaN();
publisher->msg_.wrench.torque.z = std::numeric_limits<double>::quiet_NaN();
}
}

return controller_interface::return_type::OK;
Expand Down