diff --git a/include/actionlib/client/action_client.h b/include/actionlib/client/action_client.h index f87f852c..83f93422 100644 --- a/include/actionlib/client/action_client.h +++ b/include/actionlib/client/action_client.h @@ -225,11 +225,14 @@ class ActionClient if (pub_queue_size < 0) {pub_queue_size = 10;} if (sub_queue_size < 0) {sub_queue_size = 1;} - status_sub_ = queue_subscribe("status", static_cast(sub_queue_size), + bool feedback_sub_use_tcpnodelay; + n_.param("actionlib_client_feedback_sub_tcpnodelay", feedback_sub_use_tcpnodelay, false); + + status_sub_ = queue_subscribe("status", static_cast(sub_queue_size), false, &ActionClientT::statusCb, this, queue); feedback_sub_ = queue_subscribe("feedback", static_cast(sub_queue_size), - &ActionClientT::feedbackCb, this, queue); - result_sub_ = queue_subscribe("result", static_cast(sub_queue_size), + feedback_sub_use_tcpnodelay, &ActionClientT::feedbackCb, this, queue); + result_sub_ = queue_subscribe("result", static_cast(sub_queue_size), false, &ActionClientT::resultCb, this, queue); connection_monitor_.reset(new ConnectionMonitor(feedback_sub_, result_sub_)); @@ -264,13 +267,15 @@ class ActionClient } template - ros::Subscriber queue_subscribe(const std::string & topic, uint32_t queue_size, void (T::* fp)( - const ros::MessageEvent &), T * obj, ros::CallbackQueueInterface * queue) + ros::Subscriber queue_subscribe(const std::string & topic, uint32_t queue_size, + bool use_tcpnodelay, void (T::* fp)(const ros::MessageEvent &), + T * obj, ros::CallbackQueueInterface * queue) { ros::SubscribeOptions ops; ops.callback_queue = queue; ops.topic = topic; ops.queue_size = queue_size; + ops.transport_hints.tcpNoDelay(use_tcpnodelay); ops.md5sum = ros::message_traits::md5sum(); ops.datatype = ros::message_traits::datatype(); ops.helper = ros::SubscriptionCallbackHelperPtr( diff --git a/src/actionlib/action_client.py b/src/actionlib/action_client.py index a5df2799..fb7c63eb 100755 --- a/src/actionlib/action_client.py +++ b/src/actionlib/action_client.py @@ -531,9 +531,10 @@ def __init__(self, ns, ActionSpec): self.sub_queue_size = rospy.get_param('actionlib_client_sub_queue_size', -1) if self.sub_queue_size < 0: self.sub_queue_size = None + self.feedback_sub_use_tcpnodelay = rospy.get_param('actionlib_client_feedback_sub_tcpnodelay', False) self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status', GoalStatusArray, callback=self._status_cb, queue_size=self.sub_queue_size) self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result', self.ActionResult, callback=self._result_cb, queue_size=self.sub_queue_size) - self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, callback=self._feedback_cb, queue_size=self.sub_queue_size) + self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, callback=self._feedback_cb, queue_size=self.sub_queue_size, tcp_nodelay=self.feedback_sub_use_tcpnodelay) ## @brief Sends a goal to the action server ##