|
From: <tpr...@us...> - 2009-06-30 01:02:32
|
Revision: 17926
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17926&view=rev
Author: tpratkanis
Date: 2009-06-30 00:44:22 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
Change robot actions to use the new ros::NodeHandle API.
Modified Paths:
--------------
pkg/trunk/highlevel/test_robot_actions/test/utest.cc
pkg/trunk/stacks/common/robot_actions/include/robot_actions/action.h
pkg/trunk/stacks/common/robot_actions/include/robot_actions/action_client.h
pkg/trunk/stacks/common/robot_actions/include/robot_actions/action_runner.h
pkg/trunk/stacks/common/robot_actions/include/robot_actions/message_adapter.h
pkg/trunk/stacks/common/robot_actions/src/action_runner.cpp
Modified: pkg/trunk/highlevel/test_robot_actions/test/utest.cc
===================================================================
--- pkg/trunk/highlevel/test_robot_actions/test/utest.cc 2009-06-30 00:35:56 UTC (rev 17925)
+++ pkg/trunk/highlevel/test_robot_actions/test/utest.cc 2009-06-30 00:44:22 UTC (rev 17926)
@@ -5,6 +5,7 @@
#include <std_msgs/Float32.h>
#include <test_robot_actions/TestState.h>
#include <gtest/gtest.h>
+#include <ros/ros.h>
using namespace robot_actions;
using namespace test_robot_actions;
@@ -296,16 +297,22 @@
}
+void spinThread() {
+ ros::spin();
+}
+
int main(int argc, char** argv){
- ros::init(argc, argv);
-
- ros::Node node("robot_actions_test");
+ ros::init(argc, argv, "robot_actions_test");
+ ros::NodeHandle n;
+
+ boost::thread* spinthread = new boost::thread(boost::bind(&spinThread));
+
testing::InitGoogleTest(&argc, argv);
int result = RUN_ALL_TESTS();
- node.shutdown();
+ delete spinthread;
return result;
}
Modified: pkg/trunk/stacks/common/robot_actions/include/robot_actions/action.h
===================================================================
--- pkg/trunk/stacks/common/robot_actions/include/robot_actions/action.h 2009-06-30 00:35:56 UTC (rev 17925)
+++ pkg/trunk/stacks/common/robot_actions/include/robot_actions/action.h 2009-06-30 00:44:22 UTC (rev 17926)
@@ -49,10 +49,11 @@
#include <robot_actions/ActionStatus.h>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
-#include <ros/node.h>
#include <diagnostic_msgs/DiagnosticMessage.h>
-#include "ros/assert.h"
+#include "ros/assert.h"
+#include "ros/ros.h"
+
namespace robot_actions {
/**
@@ -192,7 +193,8 @@
Action(const std::string& name)
: _name(name), _preempt_request(false), _result_status(SUCCESS), _terminated(false), _action_thread(NULL), _callback(NULL),_diagnostics_statuses(1){
_status.value = ActionStatus::RESET;
- ros::Node::instance()->advertise<diagnostic_msgs::DiagnosticMessage> ("/diagnostics", 1) ;
+ _node = new ros::NodeHandle();
+ _diagnostics_publisher = _node->advertise<diagnostic_msgs::DiagnosticMessage> ("/diagnostics", 1) ;
}
virtual ~Action(){
@@ -201,7 +203,7 @@
_action_thread->join();
delete _action_thread;
}
- ros::Node::instance()->unadvertise("/diagnostics") ;
+ delete _node;
}
/**
@@ -292,7 +294,7 @@
_diagnostics_statuses[0] = diagnostics_status;
_diagnostics_message.status = _diagnostics_statuses;
_diagnostics_message.header.stamp = ros::Time::now();
- ros::Node::instance()->publish("/diagnostics",_diagnostics_message);
+ _diagnostics_publisher.publish(_diagnostics_message);
}
@@ -309,6 +311,8 @@
boost::function< void(const ActionStatus&, const Goal&, const Feedback&) > _callback; /*!< Callback function for sending updates */
diagnostic_msgs::DiagnosticMessage _diagnostics_message;
std::vector<diagnostic_msgs::DiagnosticStatus> _diagnostics_statuses;
+ ros::NodeHandle *_node;
+ ros::Publisher _diagnostics_publisher;
};
}
#endif
Modified: pkg/trunk/stacks/common/robot_actions/include/robot_actions/action_client.h
===================================================================
--- pkg/trunk/stacks/common/robot_actions/include/robot_actions/action_client.h 2009-06-30 00:35:56 UTC (rev 17925)
+++ pkg/trunk/stacks/common/robot_actions/include/robot_actions/action_client.h 2009-06-30 00:44:22 UTC (rev 17926)
@@ -78,31 +78,41 @@
private:
- void callbackHandler(); // Just a no-op
+ void callbackHandler(const boost::shared_ptr<State const> &input); // Just a no-op
State _state_update_msg;
const std::string _goal_topic;
const std::string _preempt_topic;
const std::string _feedback_topic;
bool _is_active;
+ ros::NodeHandle _node;
+ ros::Publisher _goal_publisher, _preempt_publisher;
+ ros::Subscriber _feedback_subscriber;
};
template <class Goal, class State, class Feedback>
+ void ActionClient<Goal, State, Feedback>::callbackHandler(const boost::shared_ptr<State const>& input) {
+ _state_update_msg = *input;
+ _is_active = (_state_update_msg.status.value == _state_update_msg.status.ACTIVE);
+ }
+
+
+ template <class Goal, class State, class Feedback>
ActionClient<Goal, State, Feedback>::ActionClient(const std::string& name)
: _goal_topic(name + "/activate"), _preempt_topic(name + "/preempt"), _feedback_topic(name + "/feedback"),_is_active(false){
// Subscribe to state updates
- ros::Node::instance()->subscribe(_feedback_topic, _state_update_msg, &ActionClient<Goal, State, Feedback>::callbackHandler, this, 1);
+ _feedback_subscriber = _node.subscribe(_feedback_topic, 1, &ActionClient<Goal, State, Feedback>::callbackHandler, this);
// Advertize goal requests.
- ros::Node::instance()->advertise<Goal>(_goal_topic, 1);
+ _goal_publisher = _node.advertise<Goal>(_goal_topic, 1);
// Advertize goal preemptions.
- ros::Node::instance()->advertise<std_msgs::Empty>(_preempt_topic, 1);
+ _preempt_publisher = _node.advertise<std_msgs::Empty>(_preempt_topic, 1);
// wait until we have at least 1 subscriber per advertised topic
ros::Time start_time = ros::Time::now();
ros::Duration timeout(10.0);
- while (ros::Node::instance()->numSubscribers(_goal_topic) < 1 || ros::Node::instance()->numSubscribers(_preempt_topic) < 1 ){
+ while (_goal_publisher.getNumSubscribers() < 1 || _preempt_publisher.getNumSubscribers() < 1 ){
if (ros::Time::now() > start_time+timeout){
ROS_ERROR("Action client did not receive subscribers on the %s or %s topic", _goal_topic.c_str(), _preempt_topic.c_str());
break;
@@ -113,9 +123,6 @@
template <class Goal, class State, class Feedback>
ActionClient<Goal, State, Feedback>::~ActionClient(){
- ros::Node::instance()->unadvertise(_goal_topic);
- ros::Node::instance()->unadvertise(_preempt_topic);
- ros::Node::instance()->unsubscribe(_state_update_msg);
}
template <class Goal, class State, class Feedback>
@@ -127,7 +134,7 @@
ResultStatus result = robot_actions::PREEMPTED;
// Post the goal
- ros::Node::instance()->publish(_goal_topic, goal);
+ _goal_publisher.publish(goal);
// Wait for activation. If we do not activate in time, we will preempt. Will not block for preemption
// callback since the action may be bogus.
@@ -140,7 +147,7 @@
if(duration_bound != NO_DURATION_BOUND){
ros::Duration elapsed_time = ros::Time::now() - start_time;
if(elapsed_time > duration_bound){
- ros::Node::instance()->publish(_preempt_topic, std_msgs::Empty());
+ _preempt_publisher.publish(std_msgs::Empty());
return result;
}
}
@@ -153,7 +160,7 @@
if(duration_bound != NO_DURATION_BOUND){
ros::Duration elapsed_time = ros::Time::now() - start_time;
if(elapsed_time > duration_bound){
- ros::Node::instance()->publish(_preempt_topic, std_msgs::Empty());
+ _preempt_publisher.publish(std_msgs::Empty());
}
}
@@ -173,15 +180,10 @@
ResultStatus ActionClient<Goal, State, Feedback>::execute(const Goal& goal, Feedback& feedback){
return execute(goal, feedback, ros::Duration());
}
-
- template <class Goal, class State, class Feedback>
- void ActionClient<Goal, State, Feedback>::callbackHandler(){
- _is_active = (_state_update_msg.status.value == _state_update_msg.status.ACTIVE);
- }
template <class Goal, class State, class Feedback>
void ActionClient<Goal, State, Feedback>::preempt(){
- ros::Node::instance()->publish(_preempt_topic, std_msgs::Empty());
+ _preempt_publisher.publish(std_msgs::Empty());
}
}
Modified: pkg/trunk/stacks/common/robot_actions/include/robot_actions/action_runner.h
===================================================================
--- pkg/trunk/stacks/common/robot_actions/include/robot_actions/action_runner.h 2009-06-30 00:35:56 UTC (rev 17925)
+++ pkg/trunk/stacks/common/robot_actions/include/robot_actions/action_runner.h 2009-06-30 00:44:22 UTC (rev 17926)
@@ -40,6 +40,7 @@
#include <robot_actions/message_adapter.h>
#include <boost/thread.hpp>
#include <ros/console.h>
+#include "ros/ros.h"
namespace robot_actions {
@@ -112,6 +113,7 @@
double _update_rate; /*!< The duration for each control cycle */
boost::thread* _update_thread; /*!< Thread running the planner loop */
std::vector<AbstractAdapter*> _adapters; /*!< Collection of action adapters to run */
+ ros::NodeHandle _node;
};
}
Modified: pkg/trunk/stacks/common/robot_actions/include/robot_actions/message_adapter.h
===================================================================
--- pkg/trunk/stacks/common/robot_actions/include/robot_actions/message_adapter.h 2009-06-30 00:35:56 UTC (rev 17925)
+++ pkg/trunk/stacks/common/robot_actions/include/robot_actions/message_adapter.h 2009-06-30 00:44:22 UTC (rev 17926)
@@ -41,7 +41,7 @@
#include <robot_actions/ActionStatus.h>
#include <std_msgs/Empty.h>
#include <boost/thread.hpp>
-#include <ros/node.h>
+#include <ros/ros.h>
#include <ros/console.h>
#include <boost/thread.hpp>
@@ -128,22 +128,19 @@
_action.connect(boost::bind(&MessageAdapter<Goal, State, Feedback>::notify, this, _1, _2, _3));
// Advertize state updates
- ros::Node::instance()->advertise<State>(_feedback_topic, QUEUE_MAX());
+ _feedback_publisher = _node.advertise<State>(_feedback_topic, QUEUE_MAX());
// Subscribe to goal requests.
- ros::Node::instance()->subscribe(_goal_topic, _request_msg, &MessageAdapter<Goal, State, Feedback>::requestHandler, this, 1);
+ _goal_subscriber = _node.subscribe<Goal>(_goal_topic, 1, &MessageAdapter<Goal, State, Feedback>::requestHandler, this);
// Subscribe to goal preemptions.
- ros::Node::instance()->subscribe(_preempt_topic, _preemption_msg, &MessageAdapter<Goal, State, Feedback>::preemptionHandler, this, 1);
+ _preempt_subscriber = _node.subscribe<std_msgs::Empty>(_preempt_topic, 1, &MessageAdapter<Goal, State, Feedback>::preemptionHandler, this);
}
/**
* @brief Do ROS cleanup
*/
virtual ~MessageAdapter(){
- ros::Node::instance()->unsubscribe(_request_msg);
- ros::Node::instance()->unsubscribe(_preemption_msg);
- ros::Node::instance()->unadvertise(_feedback_topic);
}
// Call back invoked from the action. Packages up as a state message and ships
@@ -152,7 +149,7 @@
state_msg.status = status;
state_msg.goal = goal;
state_msg.feedback = feedback;
- ros::Node::instance()->publish(_feedback_topic, state_msg);
+ _feedback_publisher.publish(state_msg);
}
virtual void publish(){
@@ -172,7 +169,8 @@
/**
* @brief Handle a new request.
*/
- void requestHandler(){
+ void requestHandler(const boost::shared_ptr<Goal const> &input){
+ _request_msg = *input;
if(isOk()){
_action.activate(_request_msg);
}
@@ -181,7 +179,8 @@
/**
* @brief Handle a preemption. Will ignore the request unless the action is currently active. Delegates to subclass for details.
*/
- void preemptionHandler(){
+ void preemptionHandler(const boost::shared_ptr<std_msgs::Empty const> &input){
+ _preemption_msg = *input;
if(isOk())
_action.preempt();
}
@@ -199,6 +198,9 @@
Goal _request_msg; /*!< Message populated by handler for a request */
std_msgs::Empty _preemption_msg; /*!< Message populated by handler for a preemption. */
State _state_msg; /*!< Message published. */
+ ros::NodeHandle _node; /*!< Node handle */
+ ros::Subscriber _goal_subscriber, _preempt_subscriber; /*!< Subscribers */
+ ros::Publisher _feedback_publisher; /*!< publisher */
};
}
Modified: pkg/trunk/stacks/common/robot_actions/src/action_runner.cpp
===================================================================
--- pkg/trunk/stacks/common/robot_actions/src/action_runner.cpp 2009-06-30 00:35:56 UTC (rev 17925)
+++ pkg/trunk/stacks/common/robot_actions/src/action_runner.cpp 2009-06-30 00:44:22 UTC (rev 17926)
@@ -39,7 +39,6 @@
namespace robot_actions {
ActionRunner::ActionRunner(double update_rate): _initialized(false), _terminated(false), _update_rate(update_rate), _update_thread(NULL){
-
ROS_ASSERT(_update_rate > 0);
// Start the action_runner_thread
@@ -82,7 +81,7 @@
}
void ActionRunner::updateLoop(){
- while(ros::Node::instance()->ok() && !isTerminated()) {
+ while(_node.ok() && !isTerminated()) {
ros::Time curr = ros::Time::now();
if(_initialized){
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|