[Personalrobots-commit] SF.net SVN: personalrobots:[17926] pkg/trunk

[<tpr...@us...>]

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.