ConnectionMonitor connectionMonitor; // Have to destroy subscribers and publishers before the connectionMonitor, since we call callbacks in the connectionMonitor
/**
* \brief Simple constructor
*
* Constructs an ActionClient and sets up the necessary ros topics for the ActionInterface
* \param name The action name. Defines the namespace in which the action communicates
* \param queue CallbackQueue from which this action will process messages.
* The default (NULL) is to use the global queue
*/
public ActionClient(string name, CallbackQueueInterface queue = null)
{
nodeHandle = new NodeHandle(name);
guard_ = new DestructionGuard();
goalManager = new GoalManager <ActionSpec> (guard_);
initClient(queue);
}
public SubscriberCallbacks(SubscriberStatusCallback connectCB, SubscriberStatusCallback disconnectCB,
CallbackQueueInterface Callback)
{
connect = connectCB;
disconnect = disconnectCB;
this.Callback = Callback;
}
public SubscriberCallbacks(SubscriberStatusCallback connectCB, SubscriberStatusCallback disconnectCB,
CallbackQueueInterface Callback)
{
connect = connectCB;
disconnect = disconnectCB;
this.Callback = Callback;
}
void initClient(CallbackQueueInterface queue)
{
// assume this means wait until ros is good to go?
// ros::Time::waitForValid ();
// read parameters indicating publish/subscribe queue sizes
int pub_queue_size = 10;
int sub_queue_size = 1;
// nodeHandle.param ( "actionlib_client_pub_queue_size", pub_queue_size, 10 );
// nodeHandle.param ( "actionlib_client_sub_queue_size", sub_queue_size, 1 );
if (pub_queue_size < 0)
{
pub_queue_size = 10;
}
if (sub_queue_size < 0)
{
sub_queue_size = 1;
}
statusSubscriber = queue_subscribe <gsa> ("status", (uint)sub_queue_size, statusCb, queue);
feedbackSubscriber = queue_subscribe <ActionFeedbackDecorator> ("feedback", (uint)sub_queue_size, feedbackCb, queue);
resultSubscriber = queue_subscribe <ActionResultDecorator> ("result", (uint)sub_queue_size, resultCb, queue);
connectionMonitor = new ConnectionMonitor(feedbackSubscriber, resultSubscriber);
// connectionMonitor.reset ( new ConnectionMonitor ( feedbackSubscriber, resultSubscriber ) );
// Start publishers and subscribers
goalPublisher = queue_advertise <ActionGoalDecorator> ("goal", (uint)pub_queue_size, connectionMonitor.goalConnectCallback, connectionMonitor.goalDisconnectCallback, queue);
// goalPublisher = queue_advertise<ActionGoal>( "goal", (uint) pub_queue_size,
// boost::bind(&ConnectionMonitor::goalConnectCallback, connectionMonitor, _1),
// boost::bind(&ConnectionMonitor::goalDisconnectCallback, connectionMonitor, _1),
// queue);
cancelPublisher = queue_advertise <goalid> ("cancel", (uint)pub_queue_size, connectionMonitor.cancelConnectCallback, connectionMonitor.cancelDisconnectCallback, queue);
// cancelPublisher = queue_advertise<goalid>("cancel", static_cast<uint32_t>(pub_queue_size),
// boost::bind(&ConnectionMonitor::cancelConnectCallback, connectionMonitor, _1),
// boost::bind(&ConnectionMonitor::cancelDisconnectCallback, connectionMonitor, _1),
// queue);
goalManager.registerSendGoalDelegate(SendGoalFunc);
// goalManager.registerSendGoalDelegate(boost::bind(&ActionClient<ActionSpec>::SendGoalDelegate, this, _1));
goalManager.registerCancelFunc(sendCancelFunc);
// goalManager.registerCancelFunc(boost::bind(&ActionClient<ActionSpec>::sendCancelFunc, this, _1));
}
Subscriber <MessageEvent <M> > queue_subscribe <M> (string topic, uint queue_size, MessageEventDelegate <M> msgEvent, CallbackQueueInterface queue) where M : IRosMessage, new()
// Subscriber<M, T> queue_subscribe (string topic, uint queue_size, void(T::*fp)(const ros::MessageEvent<M const>&), T* obj, ros::CallbackQueueInterface* queue)
{
SubscribeOptions <MessageEvent <M> > ops = new SubscribeOptions <MessageEvent <M> > (topic, queue_size, new CallbackDelegate <MessageEvent <M> > (msgEvent));
ops.callback_queue = queue;
ops.topic = topic;
ops.queue_size = queue_size;
// md5 and datatype get generated in constructor
// ops.md5sum = ros::message_traits::md5sum<M>();
// ops.datatype = ros::message_traits::datatype<M>();
ops.helper = new SubscriptionCallbackHelper <MessageEvent <M> > (Messages.MsgTypes.MessageEvent, new CallbackDelegate <MessageEvent <M> > (msgEvent));
// ops.helper = ros::SubscriptionCallbackHelperPtr(
// new ros::SubscriptionCallbackHelperT<const ros::MessageEvent<M const>& >(
// boost::bind(fp, obj, _1)
// )
// );
return(nodeHandle.subscribe(ops));
}
Publisher <M> queue_advertise <M> (string topic, uint queue_size, SubscriberStatusCallback connect_cb, SubscriberStatusCallback disconnect_cb, CallbackQueueInterface queue) where M : IRosMessage, new()
{
AdvertiseOptions <M> ops = new AdvertiseOptions <M> (topic, (int)queue_size, connect_cb, disconnect_cb);
// ops.tracked_object = ros::VoidPtr();
ops.latch = false;
ops.callback_queue = queue;
return(nodeHandle.advertise(ops));
}
