Skip to content

Commit

Permalink
Add publication of forces on additional frames.
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl committed Nov 17, 2021
1 parent ce9703b commit f893c57
Show file tree
Hide file tree
Showing 4 changed files with 132 additions and 37 deletions.
16 changes: 13 additions & 3 deletions force_torque_sensor_broadcaster/CMakeLists.txt
View file
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
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
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
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>

#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

0 comments on commit f893c57

Please sign in to comment.