rtt_action_server.h
Go to the documentation of this file.
00001 #ifndef __RTT_ACTION_SERVER_H
00002 #define __RTT_ACTION_SERVER_H
00003 
00004 #include <rtt/RTT.hpp>
00005 #include <rtt/os/Timer.hpp>
00006 #include <rtt/plugin/ServicePlugin.hpp>
00007 #include <rtt/internal/GlobalService.hpp>
00008 
00009 #include <actionlib/action_definition.h>
00010 #include <actionlib/server/action_server_base.h>
00011 
00012 #include <rtt_actionlib/rtt_actionlib.h>
00013 
00014 #include <rtt_rosclock/rtt_rosclock.h>
00015 #include <rtt_roscomm/rtt_rostopic.h>
00016 
00017 namespace rtt_actionlib {
00018 
00019   // Forward declaration
00020   template<class ActionSpec>
00021   class RTTActionServer;
00022 
00024   template<class ActionSpec>
00025   class RTTActionServerStatusTimer : public RTT::os::Timer 
00026   {
00027   public:
00028     RTTActionServerStatusTimer(RTTActionServer<ActionSpec> &server, int scheduler = ORO_SCHED_OTHER) :
00029       RTT::os::Timer(1,-1),
00030       server_(server) 
00031     { 
00032       mThread = new RTT::Activity(scheduler, 0, 0.0, this, server.getName()+"_status_timer");
00033       mThread->start();
00034     }
00035 
00036     virtual ~RTTActionServerStatusTimer() { }
00037 
00039     virtual void timeout(RTT::os::Timer::TimerId timer_id) {
00040       server_.publishStatus();
00041     }
00042   private:
00044     RTTActionServer<ActionSpec> &server_;
00045   };
00046 
00048   template<class ActionSpec>
00049   class RTTActionServer : public actionlib::ActionServerBase<ActionSpec>
00050   {
00051   public:
00052     // Generates typedefs that make our lives easier
00053     ACTION_DEFINITION(ActionSpec);
00054 
00055     typedef actionlib::ServerGoalHandle<ActionSpec> GoalHandle;
00056 
00058     RTTActionServer(const double status_period = 0.200, const int sched = ORO_SCHED_OTHER);
00059     RTTActionServer(const std::string name, const double status_period = 0.200, const int sched = ORO_SCHED_OTHER);
00060     virtual ~RTTActionServer();
00061 
00063     const std::string getName() const { return name_; };
00064 
00066     bool addPorts(RTT::Service::shared_ptr service,
00067         const bool create_topics = false,
00068         const std::string &topic_namespace = "");
00069 
00071     bool ready();
00072 
00074     virtual void initialize();
00075 
00077     virtual void publishResult(const actionlib_msgs::GoalStatus& status, const Result& result);
00078 
00080     virtual void publishFeedback(const actionlib_msgs::GoalStatus& status, const Feedback& feedback);
00081 
00083     virtual void publishStatus();
00084 
00085     void registerGoalOperation(RTT::OperationInterfacePart* operation) {
00086       goal_operation_ = operation; 
00087       this->registerGoalCallback(boost::bind(&RTTActionServer<ActionSpec>::goalCallbackWrapper, this, _1));
00088     }
00089 
00090     void registerCancelOperation(RTT::OperationInterfacePart* operation) {
00091       cancel_operation_ = operation; 
00092     }
00093     
00094   private:
00095 
00096     /* \brief Wrapper for action server base goalCallback
00097      * This function is called when messages arrive on the goal RTT port. It
00098      * then reads the port and calls ActionServerBase::goalCallback, which, in
00099      * turn, calls the user supplied goal callback.
00100      */
00101     void goalCallback(RTT::base::PortInterface* port);
00102     void goalCallbackWrapper(GoalHandle gh);
00103 
00104     /* \brief Wrapper for action server base cancelCallback 
00105      * This function is called when messages arrive on the goal RTT port. It
00106      * then reads the port and calls ActionServerBase::cancelCallback, which, in
00107      * turn, calls the user supplied cancel callback.
00108      */
00109     void cancelCallback(RTT::base::PortInterface* port);
00110 
00112     std::string name_;
00113 
00115     double status_period_;
00116 
00118     rtt_actionlib::ActionBridge action_bridge_;
00119 
00121     RTTActionServerStatusTimer<ActionSpec> status_timer_;
00122 
00123     RTT::OperationCaller<void(GoalHandle)> goal_operation_;
00124     RTT::OperationCaller<void(GoalHandle)> cancel_operation_;
00125   };
00126 
00127   template <class ActionSpec>
00128     RTTActionServer<ActionSpec>::RTTActionServer(const double status_period, const int sched) :
00129       actionlib::ActionServerBase<ActionSpec>(boost::function<void (GoalHandle)>(), boost::function<void (GoalHandle)>(), false),
00130       name_("action_server"),
00131       status_period_(status_period),
00132       status_timer_(*this,sched)
00133   {
00134   }
00135 
00136   template <class ActionSpec>
00137     RTTActionServer<ActionSpec>::RTTActionServer(const std::string name, const double status_period, const int sched) :
00138       actionlib::ActionServerBase<ActionSpec>(boost::function<void (GoalHandle)>(), boost::function<void (GoalHandle)>(), false),
00139       name_(name),
00140       status_period_(status_period),
00141       status_timer_(*this, sched)
00142   {
00143 
00144   }
00145 
00146   template <class ActionSpec>
00147     RTTActionServer<ActionSpec>::~RTTActionServer()
00148     {
00149 
00150     }
00151 
00152   template <class ActionSpec>
00153     void RTTActionServer<ActionSpec>::initialize()
00154     {
00155       if(this->ready()) {
00156         // Start status publish timer 
00157         if(!status_timer_.startTimer(0,status_period_)) {
00158           RTT::log(RTT::Error) << "Failed to initialize RTT Action Server: could not start status publisher timer." << RTT::endlog();
00159         }
00160       } else {
00161         RTT::log(RTT::Error) << "Failed to initialize RTT Action Server: ports not ready." << RTT::endlog();
00162       }
00163     }
00164 
00165   template <class ActionSpec>
00166     bool RTTActionServer<ActionSpec>::ready() 
00167     {
00168       return action_bridge_.allConnected();
00169     }
00170 
00171   template <class ActionSpec>
00172     bool RTTActionServer<ActionSpec>::addPorts(
00173         RTT::Service::shared_ptr service,
00174         const bool create_topics,
00175         const std::string &topic_namespace)
00176     {
00177       // Try to get existing ports from service
00178       if(!action_bridge_.setPortsFromService(service)) {
00179         // Create the ports
00180         if(!action_bridge_.createServerPorts<ActionSpec>()) {
00181           return false;
00182         }
00183       }
00184 
00185       // Add the ports to the service
00186       service->addEventPort(
00187           action_bridge_.goalInput<ActionSpec>(), 
00188           boost::bind(&RTTActionServer<ActionSpec>::goalCallback, this, _1))
00189         .doc("Actionlib goal port. Do not read from this port directly.");
00190 
00191       service->addEventPort(
00192           action_bridge_.cancelInput(), 
00193           boost::bind(&RTTActionServer<ActionSpec>::cancelCallback, this, _1))
00194         .doc("Actionlib cancel port. Do not read from this port directly.");
00195 
00196       service->addPort(action_bridge_.resultOutput<ActionSpec>())
00197         .doc("Actionlib result port. Do not write to this port directly.");
00198 
00199       service->addPort(action_bridge_.statusOutput())
00200         .doc("Actionlib result port. Do not write to this port directly.");
00201 
00202       service->addPort(action_bridge_.feedbackOutput<ActionSpec>())
00203         .doc("Actionlib result port. Do not write to this port directly.");
00204 
00205       // Create ROS topics
00206       if(create_topics) {
00207         action_bridge_.goalInput<ActionSpec>().createStream(rtt_roscomm::topic(topic_namespace+"goal"));
00208         action_bridge_.cancelInput().createStream(rtt_roscomm::topic(topic_namespace+"cancel"));
00209         action_bridge_.resultOutput<ActionSpec>().createStream(rtt_roscomm::topic(topic_namespace+"result"));
00210         action_bridge_.statusOutput().createStream(rtt_roscomm::topic(topic_namespace+"status"));
00211         action_bridge_.feedbackOutput<ActionSpec>().createStream(rtt_roscomm::topic(topic_namespace+"feedback"));
00212       }
00213 
00214       return true;
00215     }
00216 
00217   template <class ActionSpec>
00218     void RTTActionServer<ActionSpec>::goalCallback(RTT::base::PortInterface* port)
00219     {
00220       ActionGoal goal;
00221       // Read the goal from the RTT port
00222       if(action_bridge_.goalInput<ActionSpec>().read(goal) == RTT::NewData) {
00223         // The action server base class expects a shared pointer
00224         actionlib::ActionServerBase<ActionSpec>::goalCallback(
00225             boost::make_shared<const ActionGoal>(goal));
00226       }
00227     }
00228 
00229   template <class ActionSpec>
00230     void RTTActionServer<ActionSpec>::goalCallbackWrapper(GoalHandle gh)
00231     {
00232       if(goal_operation_.ready()) {
00233         goal_operation_(gh);
00234       } 
00235     }
00236 
00237   template <class ActionSpec>
00238     void RTTActionServer<ActionSpec>::cancelCallback(RTT::base::PortInterface* port)
00239     {
00240       actionlib_msgs::GoalID goal_id;
00241       // Read the goal id from the RTT port
00242       if(action_bridge_.cancelInput().read(goal_id) == RTT::NewData) {
00243         // The action server base class expects a shared pointer
00244         actionlib::ActionServerBase<ActionSpec>::cancelCallback(
00245             boost::make_shared<const actionlib_msgs::GoalID>(goal_id));
00246       }
00247     }
00248 
00249   template <class ActionSpec>
00250     void RTTActionServer<ActionSpec>::publishResult(
00251         const actionlib_msgs::GoalStatus& status,
00252         const Result& result)
00253     {
00254       ACTION_DEFINITION(ActionSpec);
00255 
00256       boost::recursive_mutex::scoped_lock lock(this->lock_);
00257 
00258       // Create the action result container
00259       ActionResult action_result;
00260       action_result.header.stamp = rtt_rosclock::host_now();
00261       action_result.status = status;
00262       action_result.result = result;
00263 
00264       //ROS_DEBUG_NAMED("actionlib", "Publishing result for goal with id: %s and stamp: %.2f", status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
00265 
00266       // Write the result to the RTT data port
00267       action_bridge_.resultOutput<ActionSpec>().write(action_result);
00268 
00269       this->publishStatus();
00270     }
00271 
00272   template <class ActionSpec>
00273     void RTTActionServer<ActionSpec>::publishFeedback(
00274         const actionlib_msgs::GoalStatus& status,
00275         const Feedback& feedback)
00276     {
00277       ACTION_DEFINITION(ActionSpec);
00278 
00279       boost::recursive_mutex::scoped_lock lock(this->lock_);
00280 
00281       // Create the action result container
00282       ActionFeedback action_feedback;
00283       action_feedback.header.stamp = rtt_rosclock::host_now();
00284       action_feedback.status = status;
00285       action_feedback.feedback = feedback;
00286 
00287       //ROS_DEBUG_NAMED("actionlib", "Publishing result for goal with id: %s and stamp: %.2f", status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
00288 
00289       // Write the feedback to the RTT data port
00290       action_bridge_.feedbackOutput<ActionSpec>().write(action_feedback);
00291     }
00292 
00293   template <class ActionSpec>
00294     void RTTActionServer<ActionSpec>::publishStatus()
00295     {
00296       boost::recursive_mutex::scoped_lock lock(this->lock_);
00297 
00298       // Build a status array
00299       actionlib_msgs::GoalStatusArray status_array;
00300 
00301       status_array.header.stamp = rtt_rosclock::host_now();
00302 
00303       status_array.status_list.resize(this->status_list_.size());
00304 
00305       unsigned int i = 0;
00306       for(typename std::list<actionlib::StatusTracker<ActionSpec> >::iterator it = this->status_list_.begin();
00307           it != this->status_list_.end();
00308           ++i)
00309       {
00310         status_array.status_list[i] = (*it).status_;
00311 
00312         // Check if the item is due for deletion from the status list
00313         if((*it).handle_destruction_time_ != ros::Time() &&
00314            (*it).handle_destruction_time_ + this->status_list_timeout_ < rtt_rosclock::host_now()){
00315           it = this->status_list_.erase(it);
00316         } else {
00317           ++it;
00318         }
00319       }
00320 
00321       // Publish the status
00322       action_bridge_.statusOutput().write(status_array);
00323     }
00324 
00325 }
00326 
00327 
00328 
00329 #endif // ifndef __RTT_ACTION_SERVER_H


rtt_actionlib
Author(s): Jonathan Bohren
autogenerated on Wed Sep 16 2015 07:00:05