openraveros: rossessionserver.cpp Source File

Go to the documentation of this file.
00001 // Software License Agreement (BSD License)
00002 // Copyright (c) 2008-2012, Rosen Diankov <rosen.diankov@gmail.com>
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 
00035 #include "openraveros.h"
00036 
00037 #include <openraveros/openrave_session.h>
00038 
00039 using namespace ros;
00040 
00041 #define REFLECT_SERVICE(srvname) \
00042     bool srvname ## _srv(srvname::Request& req, srvname::Response& res) \
00043     { \
00044         SessionState state = getstate(req); /* need separate copy in order to guarantee thread safety */ \
00045         if( !state._pserver ) { \
00046             RAVELOG_INFOA("failed to find session for service %s\n", # srvname); \
00047             return false; \
00048         } \
00049         return state._pserver->srvname ## _srv(req,res); \
00050     }
00051 
00052 class SessionServer : public boost::enable_shared_from_this<SessionServer>
00053 {
00054     class SessionState
00055     {
00056 public:
00057         SessionState() {
00058         }
00059         SessionState(boost::shared_ptr<SessionServer> psessionserver) : _psessionserver(psessionserver) {
00060         }
00061         virtual ~SessionState() {
00062             _pserver.reset();
00063             _penv.reset();
00064         }
00065 
00066         boost::shared_ptr<SessionServer> _psessionserver;
00067         boost::shared_ptr<ROSServer> _pserver;
00068         EnvironmentBasePtr _penv;
00069     };
00070 
00071     string _sessionname;
00072 public:
00073     SessionServer() : _ok(true) {
00074         _pParentEnvironment = RaveCreateEnvironment();
00075     }
00076     virtual ~SessionServer() {
00077         Destroy();
00078     }
00079 
00080     bool Init()
00081     {
00082         _ros.reset(new ros::NodeHandle());
00083 
00084         _srvSession = _ros->advertiseService("openrave_session",&SessionServer::session_callback,shared_from_this());
00085         _sessionname = _ros->resolveName("openrave_session");
00086 
00087         // advertise persistent services
00088         _mapservices["body_destroy"] = _ros->advertiseService("body_destroy",&SessionServer::body_destroy_srv,shared_from_this());
00089         _mapservices["body_enable"] = _ros->advertiseService("body_enable",&SessionServer::body_enable_srv,shared_from_this());
00090         _mapservices["body_getaabb"] = _ros->advertiseService("body_getaabb",&SessionServer::body_getaabb_srv,shared_from_this());
00091         _mapservices["body_getaabbs"] = _ros->advertiseService("body_getaabbs",&SessionServer::body_getaabbs_srv,shared_from_this());
00092         _mapservices["body_getdof"] = _ros->advertiseService("body_getdof",&SessionServer::body_getdof_srv,shared_from_this());
00093         _mapservices["body_getjointvalues"] = _ros->advertiseService("body_getjointvalues",&SessionServer::body_getjointvalues_srv,shared_from_this());
00094         _mapservices["body_setjointvalues"] = _ros->advertiseService("body_setjointvalues",&SessionServer::body_setjointvalues_srv,shared_from_this());
00095         _mapservices["body_settransform"] = _ros->advertiseService("body_settransform",&SessionServer::body_settransform_srv,shared_from_this());
00096         _mapservices["env_checkcollision"] = _ros->advertiseService("env_checkcollision",&SessionServer::env_checkcollision_srv,shared_from_this());
00097         _mapservices["env_closefigures"] = _ros->advertiseService("env_closefigures",&SessionServer::env_closefigures_srv,shared_from_this());
00098         _mapservices["env_createbody"] = _ros->advertiseService("env_createbody",&SessionServer::env_createbody_srv,shared_from_this());
00099         _mapservices["env_createplanner"] = _ros->advertiseService("env_createplanner",&SessionServer::env_createplanner_srv,shared_from_this());
00100         _mapservices["env_createproblem"] = _ros->advertiseService("env_createproblem",&SessionServer::env_createproblem_srv,shared_from_this());
00101         _mapservices["env_createrobot"] = _ros->advertiseService("env_createrobot",&SessionServer::env_createrobot_srv,shared_from_this());
00102         _mapservices["env_destroyproblem"] = _ros->advertiseService("env_destroyproblem",&SessionServer::env_destroyproblem_srv,shared_from_this());
00103         _mapservices["env_getbodies"] = _ros->advertiseService("env_getbodies",&SessionServer::env_getbodies_srv,shared_from_this());
00104         _mapservices["env_getbody"] = _ros->advertiseService("env_getbody",&SessionServer::env_getbody_srv,shared_from_this());
00105         _mapservices["env_getrobots"] = _ros->advertiseService("env_getrobots",&SessionServer::env_getrobots_srv,shared_from_this());
00106         _mapservices["env_loadplugin"] = _ros->advertiseService("env_loadplugin",&SessionServer::env_loadplugin_srv,shared_from_this());
00107         _mapservices["env_loadscene"] = _ros->advertiseService("env_loadscene",&SessionServer::env_loadscene_srv,shared_from_this());
00108         _mapservices["env_plot"] = _ros->advertiseService("env_plot",&SessionServer::env_plot_srv,shared_from_this());
00109         _mapservices["env_raycollision"] = _ros->advertiseService("env_raycollision",&SessionServer::env_raycollision_srv,shared_from_this());
00110         _mapservices["env_set"] = _ros->advertiseService("env_set",&SessionServer::env_set_srv,shared_from_this());
00111         _mapservices["env_triangulate"] = _ros->advertiseService("env_triangulate",&SessionServer::env_triangulate_srv,shared_from_this());
00112         _mapservices["env_wait"] = _ros->advertiseService("env_wait",&SessionServer::env_wait_srv,shared_from_this());
00113         _mapservices["planner_init"] = _ros->advertiseService("planner_init",&SessionServer::planner_init_srv,shared_from_this());
00114         _mapservices["planner_plan"] = _ros->advertiseService("planner_plan",&SessionServer::planner_plan_srv,shared_from_this());
00115         _mapservices["problem_sendcommand"] = _ros->advertiseService("problem_sendcommand",&SessionServer::problem_sendcommand_srv,shared_from_this());
00116         _mapservices["robot_controllersend"] = _ros->advertiseService("robot_controllersend",&SessionServer::robot_controllersend_srv,shared_from_this());
00117         _mapservices["robot_controllerset"] = _ros->advertiseService("robot_controllerset",&SessionServer::robot_controllerset_srv,shared_from_this());
00118         _mapservices["robot_getactivevalues"] = _ros->advertiseService("robot_getactivevalues",&SessionServer::robot_getactivevalues_srv,shared_from_this());
00119         _mapservices["robot_sensorgetdata"] = _ros->advertiseService("robot_sensorgetdata",&SessionServer::robot_sensorgetdata_srv,shared_from_this());
00120         _mapservices["robot_sensorsend"] = _ros->advertiseService("robot_sensorsend",&SessionServer::robot_sensorsend_srv,shared_from_this());
00121         _mapservices["robot_setactivedofs"] = _ros->advertiseService("robot_setactivedofs",&SessionServer::robot_setactivedofs_srv,shared_from_this());
00122         _mapservices["robot_setactivemanipulator"] = _ros->advertiseService("robot_setactivemanipulator",&SessionServer::robot_setactivemanipulator_srv,shared_from_this());
00123         _mapservices["robot_setactivevalues"] = _ros->advertiseService("robot_setactivevalues",&SessionServer::robot_setactivevalues_srv,shared_from_this());
00124         _mapservices["robot_starttrajectory"] = _ros->advertiseService("robot_starttrajectory",&SessionServer::robot_starttrajectory_srv,shared_from_this());
00125 
00126         _ok = true;
00127         _threadviewer = boost::thread(boost::bind(&SessionServer::ViewerThread, this));
00128         return true;
00129     }
00130 
00131     void Destroy()
00132     {
00133         shutdown();
00134         _srvSession.shutdown();
00135         _mapservices.clear();
00136         _ros.reset();
00137         _threadviewer.join();
00138     }
00139 
00140     virtual void shutdown()
00141     {
00142         _ok = false;
00143         boost::mutex::scoped_lock lockcreate(_mutexViewer);
00144         if( !!_penvViewer ) {
00145             if( !!_pviewer ) {
00146                 _penvViewer->Remove(_pviewer);
00147             }
00148         }
00149     }
00150 
00151     bool SetViewer(EnvironmentBasePtr penv, const string& viewer, const string& title)
00152     {
00153         boost::mutex::scoped_lock lock(_mutexViewer);
00154 
00155         // destroy the old viewer
00156         if( !!_penvViewer ) {
00157             if( !!_pviewer ) {
00158                 _penvViewer->Remove(_pviewer);
00159             }
00160             _conditionViewer.wait(lock);
00161         }
00162 
00163         ROS_ASSERT(!_penvViewer);
00164 
00165         _strviewertitle = title;
00166         _strviewer = viewer;
00167         if( viewer.size() == 0 || !penv )
00168             return false;
00169 
00170         _penvViewer = penv;
00171         _conditionViewer.wait(lock);
00172         return !!_pviewer;
00173     }
00174 
00175     EnvironmentBasePtr GetParentEnvironment() const {
00176         return _pParentEnvironment;
00177     }
00178 
00179 protected:
00180     // ross
00181     boost::shared_ptr<ros::NodeHandle> _ros;
00182     ros::ServiceServer _srvSession;
00183     map<string,ros::ServiceServer> _mapservices;
00184 
00185     map<int,SessionState> _mapsessions;
00186     boost::mutex _mutexsession;
00187     EnvironmentBasePtr _pParentEnvironment;
00188 
00189     // persistent viewer
00190     ViewerBasePtr _pviewer;
00191     boost::thread _threadviewer; 
00192     boost::mutex _mutexViewer;
00193     boost::condition _conditionViewer;
00194     EnvironmentBasePtr _penvViewer;
00195     string _strviewer, _strviewertitle;
00196 
00197     bool _ok;
00198 
00199     virtual void ViewerThread()
00200     {
00201         while(_ok) {
00202 
00203             {
00204                 usleep(1000);
00205                 boost::mutex::scoped_lock lockcreate(_mutexViewer);
00206                 if( _strviewer.size() == 0 || !_penvViewer ) {
00207                     continue;
00208                 }
00209 
00210                 _pviewer = RaveCreateViewer(_penvViewer,_strviewer);
00211                 if( !!_pviewer ) {
00212                     _penvViewer->AddViewer(_pviewer);
00213                     _pviewer->ViewerSetSize(1024,768);
00214                 }
00215 
00216                 if( !_pviewer )
00217                     _penvViewer.reset();
00218 
00219                 _conditionViewer.notify_all();
00220 
00221                 if( !_pviewer )
00222                     continue;
00223             }
00224 
00225             if( _strviewertitle.size() > 0 )
00226                 _pviewer->ViewerSetTitle(_strviewertitle);
00227 
00228             _pviewer->main(); // spin until quitfrommainloop is called
00229 
00230             boost::mutex::scoped_lock lockcreate(_mutexViewer);
00231             RAVELOG_DEBUGA("destroying viewer\n");
00232             ROS_ASSERT(_penvViewer == _pviewer->GetEnv());
00233             if( !!_pviewer ) {
00234                 _penvViewer->Remove(_pviewer);
00235             }
00236             _pviewer.reset();
00237             usleep(100000); // wait a little
00238             _penvViewer.reset();
00239             _conditionViewer.notify_all();
00240         }
00241     }
00242 
00243     template <class MReq>
00244     SessionState getstate(const MReq& req)
00245     {
00246         if( !req.__connection_header )
00247             return SessionState(shared_from_this());
00248 
00249         ros::M_string::const_iterator it = req.__connection_header->find(_sessionname);
00250         if( it == req.__connection_header->end() )
00251             return SessionState(shared_from_this());
00252 
00253         boost::mutex::scoped_lock lock(_mutexsession);
00254 
00255         int sessionid = 0;
00256         stringstream ss(it->second); ss >> sessionid;
00257         if( _mapsessions.find(sessionid) == _mapsessions.end() )
00258             return SessionState(shared_from_this());
00259         return _mapsessions[sessionid];
00260     }
00261 
00262     bool session_callback(openrave_session::Request& req, openrave_session::Response& res)
00263     {
00264         if( req.sessionid != 0 ) {
00265             boost::mutex::scoped_lock lock(_mutexsession);
00266 
00267             if( req.sessionid == -1 ) {
00268                 // destroy all sessions
00269                 RAVELOG_DEBUGA("destroying all sessions\n");
00270                 _mapsessions.clear();
00271             }
00272             else {
00273                 // destory the session
00274                 if( _mapsessions.find(req.sessionid) == _mapsessions.end() )
00275                     return false;
00276 
00277                 _mapsessions.erase(req.sessionid);
00278                 RAVELOG_INFOA("destroyed openrave session: %d\n", req.sessionid);
00279             }
00280 
00281             return true;
00282         }
00283 
00284         SessionState state(shared_from_this());
00285         int id = rand();
00286         {
00287             boost::mutex::scoped_lock lock(_mutexsession);
00288             while(id == 0 || _mapsessions.find(id) != _mapsessions.end())
00289                 id = rand();
00290             _mapsessions[id] = state; // grab it
00291         }
00292 
00293         if( req.clone_sessionid ) {
00294             // clone the environment from clone_sessionid
00295             SessionState clonestate(shared_from_this());
00296             {
00297                 boost::mutex::scoped_lock lock(_mutexsession);
00298                 clonestate = _mapsessions[req.clone_sessionid];
00299             }
00300 
00301             if( !clonestate._penv )
00302                 RAVELOG_INFOA("failed to find session %d\n", req.clone_sessionid);
00303             else
00304                 state._penv = clonestate._penv->CloneSelf(req.clone_options);
00305         }
00306 
00307         if( !state._penv ) {
00308             // cloning from parent
00309             RAVELOG_DEBUGA("cloning from parent\n");
00310             state._penv = _pParentEnvironment->CloneSelf(0);
00311         }
00312 
00313         state._pserver.reset(new ROSServer(id, boost::bind(&SessionServer::SetViewer,shared_from_this(),state._penv,_1,_2), state._penv, req.physicsengine, req.collisionchecker, req.viewer));
00314         state._penv->LoadProblem(state._pserver,"");
00315 
00316         boost::mutex::scoped_lock lock(_mutexsession);
00317         _mapsessions[id] = state;
00318         res.sessionid = id;
00319 
00320         RAVELOG_INFOA("started openrave session: %d, total: %d\n", id, _mapsessions.size());
00321         return true;
00322     }
00323 
00324     REFLECT_SERVICE(body_destroy)
00325     REFLECT_SERVICE(body_enable)
00326     REFLECT_SERVICE(body_getaabb)
00327     REFLECT_SERVICE(body_getaabbs)
00328     REFLECT_SERVICE(body_getdof)
00329     REFLECT_SERVICE(body_getjointvalues)
00330     REFLECT_SERVICE(body_setjointvalues)
00331     REFLECT_SERVICE(body_settransform)
00332     REFLECT_SERVICE(env_checkcollision)
00333     REFLECT_SERVICE(env_closefigures)
00334     REFLECT_SERVICE(env_createbody)
00335     REFLECT_SERVICE(env_createplanner)
00336     REFLECT_SERVICE(env_createproblem)
00337     REFLECT_SERVICE(env_createrobot)
00338     REFLECT_SERVICE(env_destroyproblem)
00339     REFLECT_SERVICE(env_getbodies)
00340     REFLECT_SERVICE(env_getbody)
00341     REFLECT_SERVICE(env_getrobots)
00342     REFLECT_SERVICE(env_loadplugin)
00343     REFLECT_SERVICE(env_loadscene)
00344     REFLECT_SERVICE(env_plot)
00345     REFLECT_SERVICE(env_raycollision)
00346     REFLECT_SERVICE(env_set)
00347     REFLECT_SERVICE(env_triangulate)
00348     REFLECT_SERVICE(env_wait)
00349     REFLECT_SERVICE(planner_init)
00350     REFLECT_SERVICE(planner_plan)
00351     REFLECT_SERVICE(problem_sendcommand)
00352     REFLECT_SERVICE(robot_controllersend)
00353     REFLECT_SERVICE(robot_controllerset)
00354     REFLECT_SERVICE(robot_getactivevalues)
00355     REFLECT_SERVICE(robot_sensorgetdata)
00356     REFLECT_SERVICE(robot_sensorsend)
00357     REFLECT_SERVICE(robot_setactivedofs)
00358     REFLECT_SERVICE(robot_setactivemanipulator)
00359     REFLECT_SERVICE(robot_setactivevalues)
00360     REFLECT_SERVICE(robot_starttrajectory)
00361 };
00362 
00363 // check that message constants match OpenRAVE constants
00364 BOOST_STATIC_ASSERT((int)Clone_Bodies==(int)openrave_session::Request::CloneBodies);
00365 BOOST_STATIC_ASSERT((int)Clone_Viewer==(int)openrave_session::Request::CloneViewer);
00366 BOOST_STATIC_ASSERT((int)Clone_Simulation==(int)openrave_session::Request::CloneSimulation);
00367 BOOST_STATIC_ASSERT((int)Clone_RealControllers==(int)openrave_session::Request::CloneRealControllers);
00368 
00369 BOOST_STATIC_ASSERT((int)ActiveDOFs::DOF_X==(int)RobotBase::DOF_X);
00370 BOOST_STATIC_ASSERT((int)ActiveDOFs::DOF_Y==(int)RobotBase::DOF_Y);
00371 BOOST_STATIC_ASSERT((int)ActiveDOFs::DOF_Z==(int)RobotBase::DOF_Z);
00372 BOOST_STATIC_ASSERT((int)ActiveDOFs::DOF_RotationAxis==(int)RobotBase::DOF_RotationAxis);
00373 BOOST_STATIC_ASSERT((int)ActiveDOFs::DOF_Rotation3D==(int)RobotBase::DOF_Rotation3D);
00374 BOOST_STATIC_ASSERT((int)ActiveDOFs::DOF_RotationQuat==(int)RobotBase::DOF_RotationQuat);
00375 
00376 BOOST_STATIC_ASSERT((int)env_checkcollision::Request::CO_Distance==(int)CO_Distance);
00377 BOOST_STATIC_ASSERT((int)env_checkcollision::Request::CO_UseTolerance==(int)CO_UseTolerance);
00378 BOOST_STATIC_ASSERT((int)env_checkcollision::Request::CO_Contacts==(int)CO_Contacts);
00379 BOOST_STATIC_ASSERT((int)env_checkcollision::Request::CO_RayAnyHit==(int)CO_RayAnyHit);