rossensorsystem.h
Go to the documentation of this file.
00001 // Software License Agreement (BSD License)
00002 // Copyright (c) 2008, Rosen Diankov
00003 // Redistribution and use in source and binary forms, with or without
00004 // modification, are permitted provided that the following conditions are met:
00005 //   * Redistributions of source code must retain the above copyright notice,
00006 //     this list of conditions and the following disclaimer.
00007 //   * Redistributions in binary form must reproduce the above copyright
00008 //     notice, this list of conditions and the following disclaimer in the
00009 //     documentation and/or other materials provided with the distribution.
00010 //   * The name of the author may not be used to endorse or promote products
00011 //     derived from this software without specific prior written permission.
00012 //
00013 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00014 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00015 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00016 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00017 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00019 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00020 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00021 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00022 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00023 // POSSIBILITY OF SUCH DAMAGE.
00024 //
00025 // \author Rosen Diankov
00026 #ifndef ROS_SENSORSYSTEM_SYSTEM
00027 #define ROS_SENSORSYSTEM_SYSTEM
00028 
00029 #include "plugindefs.h"
00030 
00031 // used to update objects through a mocap system
00032 template <typename T>
00033 class ROSSensorSystem : public SimpleSensorSystem
00034 {
00035 public:
00036  ROSSensorSystem(const std::string& xmlid, EnvironmentBasePtr penv) : SimpleSensorSystem(xmlid,penv), _bDestroyThread(true)
00037     {
00038     }
00039     virtual ~ROSSensorSystem() {
00040         Destroy();
00041     }
00042 
00043     virtual bool Init(istream& sinput)
00044     {
00045         string cmd;
00046         _listtopics.clear();
00047         while(!sinput.eof()) {
00048             sinput >> cmd;
00049             if( !sinput )
00050                 break;
00051             std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);
00052 
00053             if( cmd == "topic" ) {
00054                 string topic;
00055                 sinput >> topic;
00056                 _listtopics.push_back(topic);
00057             }
00058             else break;
00059 
00060             if( !sinput ) {
00061                 RAVELOG_ERRORA("failed\n");
00062                 return false;
00063             }
00064         }
00065 
00066         startsubscriptions();
00067         return !!_ros;
00068     }
00069 
00070     virtual bool startsubscriptions(int queuesize=10)
00071     {
00072         Destroy();
00073 
00074         int argc=0;
00075         ros::init(argc,NULL,"openrave", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00076         if( ros::master::check() ) {
00077             _ros.reset(new ros::NodeHandle());
00078             FOREACH(ittopic,_listtopics)
00079                 _listsubtopics.push_back(_ros->subscribe(*ittopic,queuesize,&ROSSensorSystem::newdatacb,this));
00080 
00081             _bDestroyThread = false;
00082             _threadros = boost::thread(boost::bind(&ROSSensorSystem::_threadrosfn, this));
00083             return true;
00084         }
00085 
00086         return false;
00087     }
00088 
00089     virtual void Destroy()
00090     {
00091         _bDestroyThread = true;
00092         FOREACH(itsub,_listsubtopics)
00093             itsub->shutdown();
00094         _listsubtopics.clear();
00095         _ros.reset();
00096         _threadros.join();
00097     }
00098 
00099 protected:
00100     inline boost::shared_ptr<SensorSystemBase> shared_system() { return boost::static_pointer_cast<SensorSystemBase>(shared_from_this()); }
00101     inline boost::shared_ptr<SensorSystemBase const> shared_system_const() const { return boost::static_pointer_cast<SensorSystemBase const>(shared_from_this()); }
00102 
00103     virtual void _threadrosfn()
00104     {
00105         while(!_bDestroyThread) {
00106             ros::spinOnce();
00107             usleep(1000); // query every 1ms
00108         }
00109     }
00110 
00111     virtual void newdatacb(const boost::shared_ptr<T const>& topicmsg)
00112     {
00113     }
00114 
00115     boost::shared_ptr<ros::NodeHandle> _ros;
00116     boost::thread _threadros;
00117     list<ros::Subscriber> _listsubtopics;
00118     list<string> _listtopics;
00119     bool _bDestroyThread;
00120 };
00121 
00122 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


orrosplanning
Author(s): Rosen Diankov (rosen.diankov@gmail.com)
autogenerated on Sat Mar 23 2013 22:32:59