Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22240] pkg/trunk
|
From: <vij...@us...> - 2009-08-19 03:41:05
|
Revision: 22240
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22240&view=rev
Author: vijaypradeep
Date: 2009-08-19 03:40:56 +0000 (Wed, 19 Aug 2009)
Log Message:
-----------
Adding spinThread option to simple action client
Modified Paths:
--------------
pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h
Modified: pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp 2009-08-19 03:29:57 UTC (rev 22239)
+++ pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp 2009-08-19 03:40:56 UTC (rev 22240)
@@ -60,10 +60,10 @@
ROS_INFO("Got Feedback!");
}
-void spinThread()
+/*void spinThread()
{
ros::spin();
-}
+}*/
int main(int argc, char** argv)
{
@@ -71,9 +71,9 @@
ros::NodeHandle n;
- boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
+ //boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
- MoveBaseClient ac("move_base");
+ MoveBaseClient ac("move_base", true);
sleep(2.0);
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h 2009-08-19 03:29:57 UTC (rev 22239)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h 2009-08-19 03:40:56 UTC (rev 22240)
@@ -38,6 +38,7 @@
#include <boost/thread/condition.hpp>
#include "ros/ros.h"
+#include "ros/callback_queue_interface.h"
#include "actionlib/client/client_helpers.h"
namespace actionlib
@@ -69,10 +70,12 @@
*
* 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
*/
- ActionClient(const std::string& name) : n_(name)
+ ActionClient(const std::string& name, ros::CallbackQueueInterface* queue = NULL) : n_(name)
{
- initClient();
+ initClient(queue);
}
/**
@@ -82,10 +85,12 @@
* and namespaces them according the a specified NodeHandle
* \param n The node handle on top of which we want to namespace our action
* \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
*/
- ActionClient(const ros::NodeHandle& n, const std::string& name) : n_(n, name)
+ ActionClient(const ros::NodeHandle& n, const std::string& name, ros::CallbackQueueInterface* queue = NULL) : n_(n, name)
{
- initClient();
+ initClient(queue);
}
/**
@@ -196,7 +201,7 @@
cancel_pub_.publish(cancel_msg);
}
- void initClient()
+ void initClient(ros::CallbackQueueInterface* queue)
{
// Start publishers and subscribers
server_connected_ = false;
@@ -205,11 +210,21 @@
manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
- status_sub_ = n_.subscribe("status", 1, &ActionClientT::statusCb, this);
- feedback_sub_ = n_.subscribe("feedback", 1, &ActionClientT::feedbackCb, this);
- result_sub_ = n_.subscribe("result", 1, &ActionClientT::resultCb, this);
+ status_sub_ = queue_subscribe("status", 1, &ActionClientT::statusCb, this, queue);
+ feedback_sub_ = queue_subscribe("feedback", 1, &ActionClientT::feedbackCb, this, queue);
+ result_sub_ = queue_subscribe("result", 1, &ActionClientT::resultCb, this, queue);
}
+ template<class M, class T>
+ ros::Subscriber queue_subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr<M const>&), T* obj, ros::CallbackQueueInterface* queue)
+ {
+ ros::SubscribeOptions ops;
+ ops.init<M>(topic, queue_size, boost::bind(fp, obj, _1));
+ ops.transport_hints = ros::TransportHints();
+ ops.callback_queue = queue;
+ return n_.subscribe(ops);
+ }
+
void statusCb(const GoalStatusArrayConstPtr& status_array)
{
manager_.updateStatuses(status_array);
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h 2009-08-19 03:29:57 UTC (rev 22239)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h 2009-08-19 03:40:56 UTC (rev 22240)
@@ -37,8 +37,10 @@
#include <boost/thread/condition.hpp>
#include <boost/thread/mutex.hpp>
+#include <boost/scoped_ptr.hpp>
#include "ros/ros.h"
+#include "ros/callback_queue.h"
#include "actionlib/client/action_client.h"
#include "actionlib/client/simple_goal_state.h"
#include "actionlib/client/terminal_state.h"
@@ -71,10 +73,12 @@
*
* Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface
* \param name The action name. Defines the namespace in which the action communicates
+ * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false,
+ * then the user has to call ros::spin().
*/
- SimpleActionClient(const std::string& name) : ac_(name), cur_simple_state_(SimpleGoalState::PENDING)
+ SimpleActionClient(const std::string& name, bool spin_thread = false) : cur_simple_state_(SimpleGoalState::PENDING)
{
- initSimpleClient();
+ initSimpleClient(nh_, name, spin_thread);
}
/**
@@ -84,18 +88,23 @@
* the ActionInterface, and namespaces them according the a specified NodeHandle
* \param n The node handle on top of which we want to namespace our action
* \param name The action name. Defines the namespace in which the action communicates
+ * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false,
+ * then the user has to call ros::spin().
*/
- SimpleActionClient(const ros::NodeHandle& n, const std::string& name) : ac_(n, name), cur_simple_state_(SimpleGoalState::PENDING)
+ SimpleActionClient(const ros::NodeHandle& n, const std::string& name, bool spin_thread = false) : cur_simple_state_(SimpleGoalState::PENDING)
{
- initSimpleClient();
+ initSimpleClient(n, name, spin_thread);
+
}
+ ~SimpleActionClient();
+
/**
* \brief Blocks until the action server connects to this client
* \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
* \return True if the server connected in the allocated time. False on timeout
*/
- bool waitForActionServerToStart(const ros::Duration& timeout = ros::Duration(0,0) ) { return ac_.waitForActionServerToStart(timeout); }
+ bool waitForActionServerToStart(const ros::Duration& timeout = ros::Duration(0,0) ) { return ac_->waitForActionServerToStart(timeout); }
/**
* \brief Sends a goal to the ActionServer, and also registers callbacks
@@ -170,7 +179,7 @@
private:
typedef ActionClient<ActionSpec> ActionClientT;
ros::NodeHandle nh_;
- ActionClientT ac_;
+ boost::scoped_ptr<ActionClientT> ac_;
GoalHandleT gh_;
SimpleGoalState cur_simple_state_;
@@ -184,23 +193,66 @@
SimpleActiveCallback active_cb_;
SimpleFeedbackCallback feedback_cb_;
+ // Spin Thread Stuff
+ boost::mutex terminate_mutex_;
+ bool need_to_terminate_;
+ boost::thread* spin_thread_;
+ ros::CallbackQueue callback_queue;
+
// ***** Private Funcs *****
- void initSimpleClient();
+ void initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread);
void handleTransition(GoalHandleT gh);
void handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback);
void setSimpleState(const SimpleGoalState::StateEnum& next_state);
void setSimpleState(const SimpleGoalState& next_state);
+ void spinThread();
};
template<class ActionSpec>
-void SimpleActionClient<ActionSpec>::initSimpleClient()
+void SimpleActionClient<ActionSpec>::initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread)
{
+ if (spin_thread)
+ {
+ ROS_DEBUG("Spinning up a thread for the SimpleActionClient");
+ need_to_terminate_ = false;
+ spin_thread_ = new boost::thread(boost::bind(&SimpleActionClient<ActionSpec>::spinThread, this));
+ ac_.reset(new ActionClientT(n, name, &callback_queue));
+ }
+ else
+ ac_.reset(new ActionClientT(n, name));
+}
+template<class ActionSpec>
+SimpleActionClient<ActionSpec>::~SimpleActionClient()
+{
+ if (spin_thread_)
+ {
+ {
+ boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+ need_to_terminate_ = true;
+ }
+ spin_thread_->join();
+ delete spin_thread_;
+ }
}
template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::spinThread()
+{
+ while (nh_.ok())
+ {
+ {
+ boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+ if (need_to_terminate_)
+ break;
+ }
+ callback_queue.callAvailable();
+ }
+}
+
+template<class ActionSpec>
void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState::StateEnum& next_state)
{
setSimpleState( SimpleGoalState(next_state) );
@@ -232,8 +284,8 @@
cur_simple_state_ = SimpleGoalState::PENDING;
// Send the goal to the ActionServer
- gh_ = ac_.sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
- boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
+ gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
+ boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
}
template<class ActionSpec>
@@ -289,13 +341,13 @@
template<class ActionSpec>
void SimpleActionClient<ActionSpec>::cancelAllGoals()
{
- ac_.cancelAllGoals();
+ ac_->cancelAllGoals();
}
template<class ActionSpec>
void SimpleActionClient<ActionSpec>::cancelGoalsAtAndBeforeTime(const ros::Time& time)
{
- ac_.cancelAllGoalsBeforeTime(time);
+ ac_->cancelAllGoalsBeforeTime(time);
}
template<class ActionSpec>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
