diff --git a/include/actionlib/client/action_client.h b/include/actionlib/client/action_client.h index 9b63bd34..82110c34 100644 --- a/include/actionlib/client/action_client.h +++ b/include/actionlib/client/action_client.h @@ -243,31 +243,37 @@ class ActionClient } template - ros::Subscriber queue_subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr&), T* obj, ros::CallbackQueueInterface* queue) + ros::Subscriber queue_subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const ros::MessageEvent&), T* obj, ros::CallbackQueueInterface* queue) { ros::SubscribeOptions ops; - ops.init(topic, queue_size, boost::bind(fp, obj, _1)); - ops.transport_hints = ros::TransportHints(); - ops.callback_queue = queue; + ops.topic = topic; + ops.queue_size = queue_size; + ops.md5sum = ros::message_traits::md5sum(); + ops.datatype = ros::message_traits::datatype(); + ops.helper = ros::SubscriptionCallbackHelperPtr( + new ros::SubscriptionCallbackHelperT& >( + boost::bind(fp, obj, _1) + ) + ); return n_.subscribe(ops); } - void statusCb(const actionlib_msgs::GoalStatusArrayConstPtr& status_array) + void statusCb(const ros::MessageEvent& status_array_event) { ROS_DEBUG_NAMED("actionlib", "Getting status over the wire."); if (connection_monitor_) - connection_monitor_->processStatus(status_array); - manager_.updateStatuses(status_array); + connection_monitor_->processStatus(status_array_event.getConstMessage(), status_array_event.getPublisherName()); + manager_.updateStatuses(status_array_event.getConstMessage()); } - void feedbackCb(const ActionFeedbackConstPtr& action_feedback) + void feedbackCb(const ros::MessageEvent& action_feedback) { - manager_.updateFeedbacks(action_feedback); + manager_.updateFeedbacks(action_feedback.getConstMessage()); } - void resultCb(const ActionResultConstPtr& action_result) + void resultCb(const ros::MessageEvent& action_result) { - manager_.updateResults(action_result); + manager_.updateResults(action_result.getConstMessage()); } }; diff --git a/include/actionlib/client/connection_monitor.h b/include/actionlib/client/connection_monitor.h index a39a8862..b8f2a30d 100644 --- a/include/actionlib/client/connection_monitor.h +++ b/include/actionlib/client/connection_monitor.h @@ -61,7 +61,7 @@ class ACTIONLIB_DECL ConnectionMonitor void cancelDisconnectCallback(const ros::SingleSubscriberPublisher& pub); - void processStatus(const actionlib_msgs::GoalStatusArrayConstPtr& status); + void processStatus(const actionlib_msgs::GoalStatusArrayConstPtr& status, const std::string& caller_id); bool waitForActionServerToStart(const ros::Duration& timeout = ros::Duration(0,0), const ros::NodeHandle& nh = ros::NodeHandle() ); bool isServerConnected(); diff --git a/src/connection_monitor.cpp b/src/connection_monitor.cpp index 393e1d9e..a0dff5c9 100644 --- a/src/connection_monitor.cpp +++ b/src/connection_monitor.cpp @@ -162,12 +162,10 @@ string ConnectionMonitor::cancelSubscribersString() } // ********* GoalStatus Connections ********* -void ConnectionMonitor::processStatus(const actionlib_msgs::GoalStatusArrayConstPtr& status) +void ConnectionMonitor::processStatus(const actionlib_msgs::GoalStatusArrayConstPtr& status, const std::string& cur_status_caller_id) { boost::recursive_mutex::scoped_lock lock(data_mutex_); - string cur_status_caller_id = (*(status->__connection_header))["callerid"]; - if (status_received_) { if (status_caller_id_ != cur_status_caller_id)