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

feat(controllers): add TF listener and broadcaster to controllers #174

Merged
merged 3 commits into from
Dec 5, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
3 changes: 3 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,9 @@ Release Versions:

- fix(components): remove incorrect log line (#166)
- fix(controllers): move predicate publishing rate parameter to BaseControllerInterface (#168)
- feat(controllers): add TF listener in BaseControllerInterface (#169)
- feat(controllers): add TF broadcaster in BaseControllerInterface (#170)
- test(controllers): add TF listener and broadcaster tests (#172)

## 5.0.2

Expand Down
2 changes: 1 addition & 1 deletion source/modulo_components/src/ComponentInterface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -537,7 +537,7 @@ geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ros_transform(
const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
const tf2::Duration& duration) {
if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
throw exceptions::LookupTransformException("Failed to lookup transform: To TF buffer / listener configured.");
throw exceptions::LookupTransformException("Failed to lookup transform: No TF buffer / listener configured.");
}
try {
return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,10 @@
#include <controller_interface/helpers.hpp>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>

#include <state_representation/parameters/ParameterMap.hpp>

Expand Down Expand Up @@ -310,6 +314,73 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
const std::string& service_name,
const std::function<ControllerServiceResponse(const std::string& string)>& callback);

/**
* @brief Configure a transform buffer and listener.
*/
void add_tf_listener();

/**
* @brief Look up a transform from TF.
* @param frame The desired frame of the transform
* @param reference_frame The desired reference frame of the transform
* @param time_point The time at which the value of the transform is desired
* @param duration How long to block the lookup call before failing
* @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured or
* if the lookupTransform call failed
* @return If it exists, the requested transform
*/
[[nodiscard]] state_representation::CartesianPose lookup_transform(
const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
const tf2::Duration& duration);

/**
* @brief Look up a transform from TF.
* @param frame The desired frame of the transform
* @param reference_frame The desired reference frame of the transform
* @param validity_period The validity period of the latest transform from the time of lookup in seconds
* @param duration How long to block the lookup call before failing
* @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured,
* if the lookupTransform call failed, or if the transform is too old
* @return If it exists and is still valid, the requested transform
*/
[[nodiscard]] state_representation::CartesianPose lookup_transform(
const std::string& frame, const std::string& reference_frame = "world", double validity_period = -1.0,
const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10)));

/**
*@brief Configure a transform broadcaster.
*/
void add_tf_broadcaster();

/**
* @brief Configure a static transform broadcaster.
*/
void add_static_tf_broadcaster();

/**
* @brief Send a transform to TF.
* @param transform The transform to send
*/
void send_transform(const state_representation::CartesianPose& transform);

/**
* @brief Send a vector of transforms to TF.
* @param transforms The vector of transforms to send
*/
void send_transforms(const std::vector<state_representation::CartesianPose>& transforms);

/**
* @brief Send a static transform to TF.
* @param transform The transform to send
*/
void send_static_transform(const state_representation::CartesianPose& transform);

/**
* @brief Send a vector of static transforms to TF.
* @param transforms The vector of transforms to send
*/
void send_static_transforms(const std::vector<state_representation::CartesianPose>& transforms);

/**
* @brief Getter of the Quality of Service attribute.
* @return The Quality of Service attribute
Expand All @@ -334,6 +405,12 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
*/
std::timed_mutex& get_command_mutex();

/**
* @brief Check if the node has been initialized or not.
* @return True if the node is initialized, false otherwise
*/
bool is_node_initialized() const;

private:
/**
* @brief Parameter validation function
Expand Down Expand Up @@ -447,6 +524,32 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
*/
std::string validate_service_name(const std::string& service_name, const std::string& type) const;

/**
* @brief Helper method to look up a transform from TF.
* @param frame The desired frame of the transform
* @param reference_frame The desired reference frame of the transform
* @param time_point The time at which the value of the transform is desired
* @param duration How long to block the lookup call before failing
* @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured or
* if the lookupTransform call failed
* @return If it exists, the requested transform
*/
[[nodiscard]] geometry_msgs::msg::TransformStamped lookup_ros_transform(
const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
const tf2::Duration& duration);

/**
* @brief Helper function to send a vector of transforms through a transform broadcaster
* @tparam T The type of the broadcaster (tf2_ros::TransformBroadcaster or tf2_ros::StaticTransformBroadcaster)
* @param transforms The transforms to send
* @param tf_broadcaster A pointer to a configured transform broadcaster object
* @param is_static If true, treat the broadcaster as a static frame broadcaster for the sake of log messages
*/
template<typename T>
void publish_transforms(
const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
bool is_static = false);

state_representation::ParameterMap parameter_map_;///< ParameterMap for handling parameters
std::unordered_map<std::string, bool> read_only_parameters_;
std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
Expand Down Expand Up @@ -482,6 +585,11 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
custom_output_configuration_callables_;
std::map<std::string, std::function<void(const std::string&, const std::string&)>>
custom_input_configuration_callables_;

std::shared_ptr<tf2_ros::Buffer> tf_buffer_; ///< TF buffer
std::shared_ptr<tf2_ros::TransformListener> tf_listener_; ///< TF listener
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; ///< TF broadcaster
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;///< TF static broadcaster
};

template<typename T>
Expand Down Expand Up @@ -867,4 +975,33 @@ inline void BaseControllerInterface::write_output(const std::string& name, const
write_std_output<StringPublishers, std_msgs::msg::String, std::string>(name, data);
}

template<typename T>
inline void BaseControllerInterface::publish_transforms(
const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
bool is_static) {
if (!is_node_initialized()) {
throw modulo_core::exceptions::CoreException("Failed send transform(s): Node is not initialized yet.");
}
std::string modifier = is_static ? "static " : "";
if (tf_broadcaster == nullptr) {
RCLCPP_ERROR_STREAM_THROTTLE(
this->get_node()->get_logger(), *this->get_node()->get_clock(), 1000,
"Failed to send " << modifier << "transform: No " << modifier << "TF broadcaster configured.");
return;
}
try {
std::vector<geometry_msgs::msg::TransformStamped> transform_messages;
transform_messages.reserve(transforms.size());
for (const auto& tf : transforms) {
geometry_msgs::msg::TransformStamped transform_message;
modulo_core::translators::write_message(transform_message, tf, this->get_node()->get_clock()->now());
transform_messages.emplace_back(transform_message);
}
tf_broadcaster->sendTransform(transform_messages);
} catch (const std::exception& ex) {
RCLCPP_ERROR_STREAM_THROTTLE(
this->get_node()->get_logger(), *this->get_node()->get_clock(), 1000,
"Failed to send " << modifier << "transform: " << ex.what());
}
}
}// namespace modulo_controllers
108 changes: 108 additions & 0 deletions source/modulo_controllers/src/BaseControllerInterface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,13 @@

#include <chrono>

#include <console_bridge/console.h>

#include <lifecycle_msgs/msg/state.hpp>

#include <modulo_core/exceptions.hpp>
#include <modulo_core/translators/message_readers.hpp>
#include <stdexcept>

template<class... Ts>
struct overloaded : Ts... {
Expand Down Expand Up @@ -539,6 +543,101 @@ void BaseControllerInterface::add_service(
}
}

void BaseControllerInterface::add_tf_listener() {
if (!is_node_initialized()) {
throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet.");
}
if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF buffer and listener.");
console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
this->tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_node()->get_clock());
this->tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*this->tf_buffer_);
} else {
RCLCPP_DEBUG(this->get_node()->get_logger(), "TF buffer and listener already exist.");
}
}

state_representation::CartesianPose BaseControllerInterface::lookup_transform(
const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
const tf2::Duration& duration) {
auto transform = this->lookup_ros_transform(frame, reference_frame, time_point, duration);
state_representation::CartesianPose result(frame, reference_frame);
translators::read_message(result, transform);
return result;
}

state_representation::CartesianPose BaseControllerInterface::lookup_transform(
const std::string& frame, const std::string& reference_frame, double validity_period,
const tf2::Duration& duration) {
auto transform =
this->lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration);
if (validity_period > 0.0
&& (this->get_node()->get_clock()->now() - transform.header.stamp).seconds() > validity_period) {
throw modulo_core::exceptions::LookupTransformException("Failed to lookup transform: Latest transform is too old!");
}
state_representation::CartesianPose result(frame, reference_frame);
translators::read_message(result, transform);
return result;
}

geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transform(
const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
const tf2::Duration& duration) {
if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
throw modulo_core::exceptions::LookupTransformException(
"Failed to lookup transform: No TF buffer / listener configured.");
}
try {
return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);
} catch (const tf2::TransformException& ex) {
throw modulo_core::exceptions::LookupTransformException(
std::string("Failed to lookup transform: ").append(ex.what()));
}
}

void BaseControllerInterface::add_tf_broadcaster() {
if (!is_node_initialized()) {
throw modulo_core::exceptions::CoreException("Failed to add TF broadcaster: Node is not initialized yet.");
}
if (this->tf_broadcaster_ == nullptr) {
RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster.");
console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
this->tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(get_node());
} else {
RCLCPP_DEBUG(this->get_node()->get_logger(), "TF broadcaster already exists.");
}
}

void BaseControllerInterface::add_static_tf_broadcaster() {
if (!is_node_initialized()) {
throw modulo_core::exceptions::CoreException("Failed to add static TF broadcaster: Node is not initialized yet.");
}
if (this->static_tf_broadcaster_ == nullptr) {
RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding static TF broadcaster.");
console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
this->static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(get_node());
} else {
RCLCPP_DEBUG(this->get_node()->get_logger(), "Static TF broadcaster already exists.");
}
}

void BaseControllerInterface::send_transform(const state_representation::CartesianPose& transform) {
this->send_transforms(std::vector<state_representation::CartesianPose>{transform});
}

void BaseControllerInterface::send_transforms(const std::vector<state_representation::CartesianPose>& transforms) {
this->publish_transforms(transforms, this->tf_broadcaster_);
}

void BaseControllerInterface::send_static_transform(const state_representation::CartesianPose& transform) {
this->send_static_transforms(std::vector<state_representation::CartesianPose>{transform});
}

void BaseControllerInterface::send_static_transforms(
const std::vector<state_representation::CartesianPose>& transforms) {
this->publish_transforms(transforms, this->static_tf_broadcaster_, true);
}

rclcpp::QoS BaseControllerInterface::get_qos() const {
return qos_;
}
Expand All @@ -555,4 +654,13 @@ std::timed_mutex& BaseControllerInterface::get_command_mutex() {
return command_mutex_;
}

bool BaseControllerInterface::is_node_initialized() const {
try {
get_node();
return true;
} catch (const std::runtime_error&) {
return false;
}
}

}// namespace modulo_controllers
Loading
Loading