ros_publish_activity.hpp
Go to the documentation of this file.00001 /*************************************************************************** 00002 tag: Ruben Smits Tue Nov 16 09:18:49 CET 2010 ros_publish_activity.hpp 00003 00004 ros_publish_activity.hpp - description 00005 ------------------- 00006 begin : Tue November 16 2010 00007 copyright : (C) 2010 Ruben Smits 00008 email : first.last@mech.kuleuven.be 00009 00010 *************************************************************************** 00011 * This library is free software; you can redistribute it and/or * 00012 * modify it under the terms of the GNU Lesser General Public * 00013 * License as published by the Free Software Foundation; either * 00014 * version 2.1 of the License, or (at your option) any later version. * 00015 * * 00016 * This library is distributed in the hope that it will be useful, * 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 00019 * Lesser General Public License for more details. * 00020 * * 00021 * You should have received a copy of the GNU Lesser General Public * 00022 * License along with this library; if not, write to the Free Software * 00023 * Foundation, Inc., 59 Temple Place, * 00024 * Suite 330, Boston, MA 02111-1307 USA * 00025 * * 00026 ***************************************************************************/ 00027 00028 00029 #ifndef _ROS_MSG_ACTIVITY_HPP_ 00030 #define _ROS_MSG_ACTIVITY_HPP_ 00031 00032 #include <rtt/Activity.hpp> 00033 #include <rtt/os/Mutex.hpp> 00034 #include <rtt/os/MutexLock.hpp> 00035 #include <boost/shared_ptr.hpp> 00036 #include <rtt/Logger.hpp> 00037 00038 #include <ros/ros.h> 00039 00040 namespace ros_integration{ 00041 using namespace RTT; 00042 00047 struct RosPublisher 00048 { 00049 public: 00053 virtual void publish()=0; 00054 }; 00055 00056 00065 class RosPublishActivity : public RTT::Activity { 00066 public: 00067 typedef boost::shared_ptr<RosPublishActivity> shared_ptr; 00068 private: 00069 typedef boost::weak_ptr<RosPublishActivity> weak_ptr; 00071 static weak_ptr ros_pub_act; 00072 00076 typedef std::map< RosPublisher*, bool> Publishers; 00077 Publishers publishers; 00078 os::Mutex map_lock; 00079 00080 RosPublishActivity( const std::string& name) 00081 : Activity(ORO_SCHED_OTHER, RTT::os::LowestPriority, 0.0, 0, name) 00082 { 00083 Logger::In in("RosPublishActivity"); 00084 log(Debug)<<"Creating RosPublishActivity"<<endlog(); 00085 } 00086 00087 void loop(){ 00088 os::MutexLock lock(map_lock); 00089 for(Publishers::iterator it = publishers.begin(); it != publishers.end(); ++it) 00090 if (it->second) { 00091 it->second = false; // protected by the mutex lock ! 00092 it->first->publish(); 00093 } 00094 } 00095 00096 public: 00097 00105 static shared_ptr Instance() { 00106 shared_ptr ret = ros_pub_act.lock(); 00107 if ( !ret ) { 00108 ret.reset(new RosPublishActivity("RosPublishActivity")); 00109 ros_pub_act = ret; 00110 ret->start(); 00111 } 00112 return ret; 00113 } 00114 00115 void addPublisher(RosPublisher* pub) { 00116 os::MutexLock lock(map_lock); 00117 publishers[pub] = false; 00118 } 00119 00120 void removePublisher(RosPublisher* pub) { 00121 os::MutexLock lock(map_lock); 00122 publishers.erase(pub); 00123 } 00124 00130 bool requestPublish(RosPublisher* chan){ 00131 // flag that data is available in a channel: 00132 { 00133 os::MutexLock lock(map_lock); 00134 assert(publishers.find(chan) != publishers.end() ); 00135 publishers.find(chan)->second = true; 00136 } 00137 // trigger loop() 00138 this->trigger(); 00139 return true; 00140 } 00141 ~RosPublishActivity() { 00142 Logger::In in("RosPublishActivity"); 00143 log(Info) << "RosPublishActivity cleans up: no more work."<<endlog(); 00144 stop(); 00145 } 00146 00147 };//class 00148 }//namespace 00149 #endif 00150