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

Jump to bottom

Use node interface pointers in the TransformListener class #100

Closed
wants to merge 2 commits into from
Closed

Use node interface pointers in the TransformListener class #100

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
deb4da2
Use node interface pointers in the TransformListener class
Mar 29, 2019
9b7694a
Format code based on ament_cpplint and ament_uncrustify
Apr 5, 2019
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
67 changes: 47 additions & 20 deletions tf2_ros/include/tf2_ros/transform_listener.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,65 +29,92 @@

/** \author Tully Foote */

#ifndef TF2_ROS_TRANSFORMLISTENER_H
#define TF2_ROS_TRANSFORMLISTENER_H
#ifndef TF2_ROS__TRANSFORM_LISTENER_H_
#define TF2_ROS__TRANSFORM_LISTENER_H_

#include <memory>
#include <string>
#include <thread>
#include "tf2_msgs/msg/tf_message.hpp"
#include "rclcpp/rclcpp.hpp"

#include "tf2_ros/buffer.h"
#include "tf2_ros/visibility_control.h"

namespace tf2_ros{

namespace tf2_ros
{

/** \brief This class provides an easy way to request and receive coordinate frame transform information.
*/
class TransformListener
{

public:
/**@brief Constructor for transform listener */
TF2_ROS_PUBLIC
TransformListener(tf2::BufferCore& buffer, bool spin_thread = true);

TransformListener(tf2::BufferCore & buffer, bool spin_thread = true);

TF2_ROS_PUBLIC
TransformListener(tf2::BufferCore & buffer, rclcpp::Node::SharedPtr nh, bool spin_thread = true);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this might be up for discussion, but I advocate for removing this constructor completely and thus force the usage of the node interfaces.


TF2_ROS_PUBLIC
TransformListener(tf2::BufferCore& buffer, rclcpp::Node::SharedPtr nh, bool spin_thread = true);
TransformListener(
tf2::BufferCore & buffer,
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

don't pass a shared pointer by reference.

const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics,
bool spin_thread = true);

TF2_ROS_PUBLIC
~TransformListener();

private:

/// Initialize this transform listener, subscribing, advertising services, etc.
void init();
void initThread();

template<
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this signature should not be necessary. Including this header file should be enough

typename MessageT,
typename CallbackT,
typename Alloc = std::allocator<void>,
typename SubscriptionT = rclcpp::Subscription<
typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
std::shared_ptr<SubscriptionT>
create_subscription(
const std::string & topic_name,
CallbackT && callback,
const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
bool ignore_local_publications = false,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
msg_mem_strat = nullptr,
std::shared_ptr<Alloc> allocator = nullptr);

/// Callback function for ros message subscriptoin
void subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg);
void static_subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg);
void subscription_callback_impl(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static);

// ros::CallbackQueue tf_message_callback_queue_;
std::thread* dedicated_listener_thread_;
rclcpp::Node::SharedPtr node_;
std::thread * dedicated_listener_thread_;
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;

rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr message_subscription_tf_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr message_subscription_tf_static_;
tf2::BufferCore& buffer_;
tf2::BufferCore & buffer_;
bool using_dedicated_thread_;
tf2::TimePoint last_update_;

void dedicatedListenerThread()
{
while (using_dedicated_thread_)
{
while (using_dedicated_thread_) {
break;
//TODO(tfoote) reenable callback queue processing
//tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01));
// TODO(tfoote) reenable callback queue processing
// tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01));
}
};

}
};
}
} // namespace tf2_ros

#endif //TF_TRANSFORMLISTENER_H
#endif // TF2_ROS__TRANSFORM_LISTENER_H_
139 changes: 96 additions & 43 deletions tf2_ros/src/transform_listener.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,61 +29,77 @@

/** \author Tully Foote */

#include <memory>
#include <string>
#include <utility>

#include "tf2_ros/transform_listener.h"


using namespace tf2_ros;

//TODO(tfoote replace these terrible macros)
// TODO(tfoote replace these terrible macros)
#define ROS_ERROR printf
#define ROS_FATAL printf
#define ROS_INFO printf
#define ROS_WARN printf

TransformListener::TransformListener(tf2::BufferCore& buffer, bool spin_thread):
dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false)
TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread)
: TransformListener(buffer, rclcpp::Node::make_shared("transform_listener_impl"), spin_thread)
{
//TODO(tfoote)make this anonymous
node_ = rclcpp::Node::make_shared("transform_listener_impl");
init();
if (spin_thread)
initThread();
}

TransformListener::TransformListener(tf2::BufferCore& buffer, rclcpp::Node::SharedPtr nh, bool spin_thread)
: dedicated_listener_thread_(NULL)
, node_(nh)
, buffer_(buffer)
, using_dedicated_thread_(false)
TransformListener::TransformListener(
tf2::BufferCore & buffer, rclcpp::Node::SharedPtr nh,
bool spin_thread)
: TransformListener(buffer, nh->get_node_base_interface(),
nh->get_node_topics_interface(), spin_thread)
{
}

TransformListener::TransformListener(
tf2::BufferCore & buffer,
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

don't pass a shared pointer by reference

const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics,
bool spin_thread)
: dedicated_listener_thread_(NULL),
node_base_(node_base),
node_topics_(node_topics),
buffer_(buffer),
using_dedicated_thread_(false)
{
init();
if (spin_thread)
if (spin_thread) {
initThread();
}
}


TransformListener::~TransformListener()
{
using_dedicated_thread_ = false;
if (dedicated_listener_thread_)
{
if (dedicated_listener_thread_) {
dedicated_listener_thread_->join();
delete dedicated_listener_thread_;
}
}

void test_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg){
return;
void test_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg)
{
}

void TransformListener::init()
{
rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
custom_qos_profile.depth = 100;
std::function<void(const tf2_msgs::msg::TFMessage::SharedPtr)> standard_callback = std::bind(&TransformListener::subscription_callback, this, std::placeholders::_1);
message_subscription_tf_ = node_->create_subscription<tf2_msgs::msg::TFMessage>("/tf", standard_callback, custom_qos_profile);
std::function<void(const tf2_msgs::msg::TFMessage::SharedPtr)> static_callback = std::bind(&TransformListener::static_subscription_callback, this, std::placeholders::_1);
message_subscription_tf_static_ = node_->create_subscription<tf2_msgs::msg::TFMessage>("/tf_static", static_callback, custom_qos_profile);
std::function<void(const tf2_msgs::msg::TFMessage::SharedPtr)> standard_callback = std::bind(
&TransformListener::subscription_callback, this, std::placeholders::_1);
message_subscription_tf_ = create_subscription<tf2_msgs::msg::TFMessage>("/tf", standard_callback,
custom_qos_profile);
std::function<void(const tf2_msgs::msg::TFMessage::SharedPtr)> static_callback = std::bind(
&TransformListener::static_subscription_callback, this, std::placeholders::_1);
message_subscription_tf_static_ = create_subscription<tf2_msgs::msg::TFMessage>("/tf_static",
static_callback,
custom_qos_profile);
}

void TransformListener::initThread()
Expand All @@ -93,15 +109,53 @@ void TransformListener::initThread()
// rclcpp::spin, since there are more than one versions of it (overloaded).
// see: http://stackoverflow.com/a/27389714/671658
// I (wjwwood) chose to use the lamda rather than the static cast solution.
auto run_func = [](rclcpp::Node::SharedPtr node) {
return rclcpp::spin(node);
};
dedicated_listener_thread_ = new std::thread(run_func, node_);
//Tell the buffer we have a dedicated thread to enable timeouts
auto run_func = [](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) {
return rclcpp::spin(node_base);
};
dedicated_listener_thread_ = new std::thread(run_func, node_base_);
// Tell the buffer we have a dedicated thread to enable timeouts
buffer_.setUsingDedicatedThread(true);
}

template<
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same comment as before.

typename MessageT,
typename CallbackT,
typename Alloc,
typename SubscriptionT>
std::shared_ptr<SubscriptionT>
TransformListener::create_subscription(
const std::string & topic_name,
CallbackT && callback,
const rmw_qos_profile_t & qos_profile,
rclcpp::callback_group::CallbackGroup::SharedPtr group,
bool ignore_local_publications,
typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
msg_mem_strat,
std::shared_ptr<Alloc> allocator)
{
using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;

if (!allocator) {
allocator = std::make_shared<Alloc>();
}

if (!msg_mem_strat) {
using rclcpp::message_memory_strategy::MessageMemoryStrategy;
msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, Alloc>::create_default();
}

return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, SubscriptionT>(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why not calling this function directly?

node_topics_.get(),
topic_name,
std::forward<CallbackT>(callback),
qos_profile,
nullptr,
false,
false,
msg_mem_strat,
allocator);
}

void TransformListener::subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg)
{
Expand All @@ -112,23 +166,22 @@ void TransformListener::static_subscription_callback(const tf2_msgs::msg::TFMess
subscription_callback_impl(msg, true);
}

void TransformListener::subscription_callback_impl(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static)
void TransformListener::subscription_callback_impl(
const tf2_msgs::msg::TFMessage::SharedPtr msg,
bool is_static)
{
const tf2_msgs::msg::TFMessage& msg_in = *msg;
//TODO(tfoote) find a way to get the authority
std::string authority = "Authority undetectable"; //msg_evt.getPublisherName(); // lookup the authority
for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
{
try
{
const tf2_msgs::msg::TFMessage & msg_in = *msg;
// TODO(tfoote) find a way to get the authority
std::string authority = "Authority undetectable"; // msg_evt.getPublisherName(); // lookup the authority
for (unsigned int i = 0; i < msg_in.transforms.size(); i++) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nitpick: for (auto i = 0u; ..)

try {
buffer_.setTransform(msg_in.transforms[i], authority, is_static);
}

catch (tf2::TransformException& ex)
{
///\todo Use error reporting
} catch (tf2::TransformException & ex) {
// /\todo Use error reporting
std::string temp = ex.what();
ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str());
ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n",
msg_in.transforms[i].child_frame_id.c_str(),
msg_in.transforms[i].header.frame_id.c_str(), temp.c_str());
}
}
};
}