Skip to content

rtt_roscomm: Shortcut ROS spinner callback queue for rtt subscribers #156

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

Open
wants to merge 5 commits into
base: toolchain-2.9
Choose a base branch
from
Open

rtt_roscomm: Shortcut ROS spinner callback queue for rtt subscribers #156

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8129810
rtt_roscomm: shortcut ROS spinner callback queue
spd-intermodalics Sep 10, 2020
13afcbe
rtt_roscomm: attend PR comments
spd-intermodalics Sep 30, 2020
66c6742
rtt_roscomm: add license to new files
spd-intermodalics Oct 1, 2020
6c94d12
rtt_roscomm: remove redundant documentation
spd-intermodalics Oct 1, 2020
3a73676
rtt_roscomm: use always ROS queue size of 1 for subscribers
spd-intermodalics Oct 1, 2020
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
252 changes: 33 additions & 219 deletions rtt_roscomm/include/rtt_roscomm/passthrough_callback_queue.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

#ifndef RTT_ROSCOMM__PASSTHROUGH_CALLBACK_QUEUE_HPP_
#define RTT_ROSCOMM__PASSTHROUGH_CALLBACK_QUEUE_HPP_

Expand All @@ -8,224 +7,39 @@
#include <ros/callback_queue.h>

namespace rtt_roscomm {
class PassthroughCallbackQueue: public ros::CallbackQueueInterface
{
public:
PassthroughCallbackQueue();

/**
* Implementation of ros::CallbackQueueInterface::addCallback()
*
* @param callback aaa
* @param owner_id aaa
*
* @return void
*/
virtual void addCallback(
const ros::CallbackInterfacePtr &callback,
uint64_t owner_id=0);

/**
* Implementation of ros::CallbackQueueInterface::removeByID()
*
* @param owner_id aaa
*
* @return void
*/
virtual void removeByID(uint64_t owner_id);

private:

/**
* Port where to write the message
*/
// base::PortInterface* output_port_ptr_;

}; // class PassthroughCallbackQueue

/**
* Helpers to generate subscribers with a custom ros::CallbackQueueInterface
*/

// template<class M, class T>
// ros::Subscriber subscribe(const std::string& topic, uint32_t queue_size,
// void(T::*fp)(const boost::shared_ptr<M const>&) const, T* obj,
// ros::CallbackQueueInterface* cq, ros::NodeHandle& nh,
// const TransportHints& transport_hints = ros::TransportHints())
// {
// ros::SubscribeOptions ops;
// ops.template init<M>(topic, queue_size, boost::bind(fp, obj, _1));
// ops.transport_hints = transport_hints;
// ops.callback_queue = cq;
// return nh.subscribe(ops);
// }

template<class M, class T>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj,
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj, _1));
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}

/// and the const version
template<class M, class T>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size, void(T::*fp)(M) const, T* obj,
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj, _1));
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M, class T>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size,
void(T::*fp)(const boost::shared_ptr<M const>&), T* obj,
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template init<M>(topic, queue_size, boost::bind(fp, obj, _1));
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M, class T>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size,
void(T::*fp)(const boost::shared_ptr<M const>&) const, T* obj,
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template init<M>(topic, queue_size, boost::bind(fp, obj, _1));
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M, class T>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size, void(T::*fp)(M),
const boost::shared_ptr<T>& obj,
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj.get(), _1));
ops.tracked_object = obj;
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}

template<class M, class T>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size, void(T::*fp)(M) const,
const boost::shared_ptr<T>& obj,
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj.get(), _1));
ops.tracked_object = obj;
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M, class T>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size,
void(T::*fp)(const boost::shared_ptr<M const>&),
const boost::shared_ptr<T>& obj,
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template init<M>(topic, queue_size, boost::bind(fp, obj.get(), _1));
ops.tracked_object = obj;
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M, class T>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size,
void(T::*fp)(const boost::shared_ptr<M const>&) const,
const boost::shared_ptr<T>& obj,
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template init<M>(topic, queue_size, boost::bind(fp, obj.get(), _1));
ops.tracked_object = obj;
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size, void(*fp)(M),
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template initByFullCallbackType<M>(topic, queue_size, fp);
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size,
void(*fp)(const boost::shared_ptr<M const>&),
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template init<M>(topic, queue_size, fp);
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size,
const boost::function<void (const boost::shared_ptr<M const>&)>& callback,
const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr(),
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template init<M>(topic, queue_size, callback);
ops.tracked_object = tracked_object;
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
template<class M, class C>
ros::Subscriber subscribe(
ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
const std::string& topic, uint32_t queue_size,
const boost::function<void (C)>& callback,
const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr(),
const ros::TransportHints& transport_hints = ros::TransportHints())
{
ros::SubscribeOptions ops;
ops.template initByFullCallbackType<C>(topic, queue_size, callback);
ops.tracked_object = tracked_object;
ops.transport_hints = transport_hints;
ops.callback_queue = cq;
return nh.subscribe(ops);
}
class PassthroughCallbackQueue: public ros::CallbackQueueInterface
{
public:
/**
* Implementation of ros::CallbackQueueInterface::addCallback()
*
* This method is executing the callback received instead of adding
* it to a queue. In this way, the queue is bypassed and immediately
* executed.
*
* @param callback callback to execute, instead of queueing
* @param owner_id Owner of the callback, in other implementations it might
* be used to remove the callback. In this implementation
* it has no effect.
*
* @return void
*/
virtual void addCallback(
const ros::CallbackInterfacePtr &callback,
uint64_t owner_id=0);

/**
* Implementation of ros::CallbackQueueInterface::removeByID()
*
* No-op
*
* @param owner_id Not used.
*
* @return void
*/
virtual void removeByID(uint64_t owner_id) {}

}; // class PassthroughCallbackQueue

} // namespace rtt_roscomm

Expand Down
21 changes: 13 additions & 8 deletions rtt_roscomm/include/rtt_roscomm/rtt_rostopic_ros_msg_transporter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,8 +148,6 @@ namespace rtt_roscomm {
}

~RosPubChannelElement() {
RTT::Logger::In in(topicname);
// RTT::log(RTT::Debug) << "Destroying RosPubChannelElement" << RTT::endlog();
act->removePublisher( this );
}

Expand Down Expand Up @@ -241,8 +239,8 @@ namespace rtt_roscomm {
std::string topicname;
ros::NodeHandle ros_node;
ros::NodeHandle ros_node_private;
PassthroughCallbackQueue passthrough_callback_queue;
ros::Subscriber ros_sub;
PassthroughCallbackQueue prassthrough_callback_queue;

public:
/**
Expand All @@ -258,23 +256,30 @@ namespace rtt_roscomm {
ros_node(),
ros_node_private("~")
{
topicname=policy.name_id;
topicname = policy.name_id;
ros::SubscribeOptions ops;
ops.template initByFullCallbackType<const RosType&>(
topicname,
policy.size > 0 ? policy.size : 1, // minimum queue_size 1
boost::bind(&RosSubChannelElement::newData, this, _1));
ops.callback_queue = &passthrough_callback_queue;

RTT::Logger::In in(topicname);
if (port->getInterface() && port->getInterface()->getOwner()) {
RTT::log(RTT::Debug)<<"Creating ROS subscriber for port "<<port->getInterface()->getOwner()->getName()<<"."<<port->getName()<<" on topic "<<policy.name_id<<RTT::endlog();
} else {
RTT::log(RTT::Debug)<<"Creating ROS subscriber for port "<<port->getName()<<" on topic "<<policy.name_id<<RTT::endlog();
}
if(topicname.length() > 1 && topicname.at(0) == '~') {
ros_sub = subscribe(ros_node_private, &prassthrough_callback_queue, policy.name_id.substr(1), policy.size > 0 ? policy.size : 1, &RosSubChannelElement::newData, this); // minimum queue_size 1
ops.topic = ops.topic.substr(1);
ros_sub = ros_node_private.subscribe(ops);
} else {
ros_sub = subscribe(ros_node, &prassthrough_callback_queue, policy.name_id, policy.size > 0 ? policy.size : 1, &RosSubChannelElement::newData, this); // minimum queue_size 1
ros_sub = ros_node.subscribe(ops);
}
}

~RosSubChannelElement() {
RTT::Logger::In in(topicname);
// RTT::log(RTT::Debug)<<"Destroying RosSubChannelElement"<<RTT::endlog();
ros_sub.shutdown();
}

virtual bool inputReady() {
Expand Down
13 changes: 2 additions & 11 deletions rtt_roscomm/src/passthrough_callback_queue.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,24 +1,15 @@

#include "rtt_roscomm/passthrough_callback_queue.hpp"

namespace rtt_roscomm {

PassthroughCallbackQueue::PassthroughCallbackQueue()
{}

void PassthroughCallbackQueue::addCallback(
const ros::CallbackInterfacePtr &callback,
uint64_t owner_id)
{
// Call CallbackInterface::CallResult SubscriptionQueue::call()
if (ros::CallbackInterface::TryAgain == callback->call()) {
ros::CallbackInterface::CallResult result = callback->call();
if (ros::CallbackInterface::TryAgain == result) {
ros::getGlobalCallbackQueue()->addCallback(callback, owner_id);
}
}

void PassthroughCallbackQueue::removeByID(uint64_t owner_id)
{
}

} // namespace rtt_roscomm