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

[<is...@us...>]

Revision: 18201
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18201&view=rev
Author:   isucan
Date:     2009-07-02 18:37:50 +0000 (Thu, 02 Jul 2009)

Log Message:
-----------
automatic selection of planners

Modified Paths:
--------------
    pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
    pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
    pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
    pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
    pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp

Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch	2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch	2009-07-02 18:37:50 UTC (rev 18201)
@@ -1,7 +1,9 @@
 <launch>
   
   <include file="$(find pr2_defs)/pr2_planning_environment.launch" />
-  <include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
+
+  <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
+  <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 4 .75 .25" />
   
   <node pkg="laser_scan" type="scan_to_cloud" output="screen">
     <param name="scan_topic" type="string" value="tilt_scan" />

Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h	2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h	2009-07-02 18:37:50 UTC (rev 18201)
@@ -67,18 +67,10 @@
 	std::map<std::string, PlannerSetup*> planners;
 	
     protected:
+
+	template<typename _T>
+	void add_planner(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
 	
-	void add_kRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	void add_kLazyRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	void add_kEST(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	void add_kSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	void add_kIKSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	void add_kKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	void add_kLBKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	void add_kIKKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	
-	void add_dRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-	void add_dKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
     };
     
     typedef std::map<std::string, Model*> ModelMap;

Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp	2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp	2009-07-02 18:37:50 UTC (rev 18201)
@@ -53,114 +53,45 @@
     for (unsigned int i = 0 ; i < cfgs.size() ; ++i)
     {
 	std::string type = cfgs[i]->getParamString("type");
+	
 	if (type == "kinematic::RRT")
-	    add_kRRT(cfgs[i]);
+	    add_planner<kinematicRRTSetup>(cfgs[i]);
 	else
 	if (type == "kinematic::LazyRRT")
-	    add_kLazyRRT(cfgs[i]);
+	    add_planner<kinematicLazyRRTSetup>(cfgs[i]);
 	else
 	if (type == "kinematic::EST")
-	    add_kEST(cfgs[i]);
+	    add_planner<kinematicESTSetup>(cfgs[i]);
 	else
 	if (type == "kinematic::SBL")
-	    add_kSBL(cfgs[i]);
+	    add_planner<kinematicSBLSetup>(cfgs[i]);
 	else
 	if (type == "kinematic::KPIECE")
-	    add_kKPIECE(cfgs[i]);
+	    add_planner<kinematicKPIECESetup>(cfgs[i]);
 	else
 	if (type == "kinematic::LBKPIECE")
-	    add_kLBKPIECE(cfgs[i]);
+	    add_planner<kinematicLBKPIECESetup>(cfgs[i]);
 	else
 	if (type == "kinematic::IKSBL")
-	    add_kIKSBL(cfgs[i]);
+	    add_planner<kinematicIKSBLSetup>(cfgs[i]);
 	else
 	if (type == "dynamic::RRT")
-	    add_dRRT(cfgs[i]);
+	    add_planner<dynamicRRTSetup>(cfgs[i]);
 	else
 	if (type == "dynamic::KPIECE")
-	    add_dKPIECE(cfgs[i]);
+	    add_planner<dynamicKPIECESetup>(cfgs[i]);
 	else
 	    ROS_WARN("Unknown planner type: %s", type.c_str());
     }
 }
 
-void ompl_planning::Model::add_dRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
+template<typename _T>
+void ompl_planning::Model::add_planner(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
 {
-    PlannerSetup *rrt = new dynamicRRTSetup(dynamic_cast<ModelBase*>(this));
-    if (rrt->setup(options))
-	planners[rrt->name] = rrt;
+    PlannerSetup *p = new _T(dynamic_cast<ModelBase*>(this));
+    if (p->setup(options))
+	planners[p->name + "[" + options->getName() + "]"] = p;
     else
-	delete rrt;
+	delete p;
 }
 
-void ompl_planning::Model::add_kRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
-    PlannerSetup *rrt = new kinematicRRTSetup(dynamic_cast<ModelBase*>(this));
-    if (rrt->setup(options))
-	planners[rrt->name] = rrt;
-    else
-	delete rrt;
-}
-
-void ompl_planning::Model::add_kLazyRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
-    PlannerSetup *rrt = new kinematicLazyRRTSetup(dynamic_cast<ModelBase*>(this));
-    if (rrt->setup(options))
-	planners[rrt->name] = rrt;
-    else
-	delete rrt;
-}
-
-void ompl_planning::Model::add_kEST(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
-    PlannerSetup *est = new kinematicESTSetup(dynamic_cast<ModelBase*>(this));
-    if (est->setup(options))
-	planners[est->name] = est;
-    else
-	delete est;
-}
-
-void ompl_planning::Model::add_kSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
-    PlannerSetup *sbl = new kinematicSBLSetup(dynamic_cast<ModelBase*>(this));
-    if (sbl->setup(options))
-	planners[sbl->name] = sbl;
-    else
-	delete sbl;
-}
-
-void ompl_planning::Model::add_kIKSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
-    PlannerSetup *sbl = new kinematicIKSBLSetup(dynamic_cast<ModelBase*>(this));
-    if (sbl->setup(options))
-	planners[sbl->name] = sbl;
-    else
-	delete sbl;
-}
-
-void ompl_planning::Model::add_kKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
-    PlannerSetup *kpiece = new kinematicKPIECESetup(dynamic_cast<ModelBase*>(this));
-    if (kpiece->setup(options))
-	planners[kpiece->name] = kpiece;
-    else
-	delete kpiece;
-}
-
-void ompl_planning::Model::add_dKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
-    PlannerSetup *kpiece = new dynamicKPIECESetup(dynamic_cast<ModelBase*>(this));
-    if (kpiece->setup(options))
-	planners[kpiece->name] = kpiece;
-    else
-	delete kpiece;
-}
-
-void ompl_planning::Model::add_kLBKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
-    PlannerSetup *kpiece = new kinematicLBKPIECESetup(dynamic_cast<ModelBase*>(this));
-    if (kpiece->setup(options))
-	planners[kpiece->name] = kpiece;
-    else
-	delete kpiece;
-}

Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp	2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp	2009-07-02 18:37:50 UTC (rev 18201)
@@ -68,7 +68,14 @@
 	    }
     
     /* check if desired planner exists */
-    std::map<std::string, PlannerSetup*>::iterator plannerIt = m->planners.find(req.params.planner_id);
+    std::map<std::string, PlannerSetup*>::iterator plannerIt = m->planners.end();
+    for (std::map<std::string, PlannerSetup*>::iterator it = m->planners.begin() ; it != m->planners.end() ; ++it)
+	if (it->first.find(req.params.planner_id) != std::string::npos)
+	{
+	    plannerIt = it;
+	    break;
+	}
+    
     if (plannerIt == m->planners.end())
     {
 	ROS_ERROR("Motion planner not found: '%s'", req.params.planner_id.c_str());
@@ -201,14 +208,14 @@
     if (!isRequestValid(models, req))
 	return false;
     
-    ROS_INFO("Selected motion planner: '%s'", req.params.planner_id.c_str());
-    
     // find the data we need 
     Model *m = models[req.params.model_id];
     
     // get the planner setup
     PlannerSetup *psetup = m->planners[req.params.planner_id];
-        
+    
+    ROS_INFO("Selected motion planner: '%s', with priority %d", req.params.planner_id.c_str(), psetup->priority);
+    
     psetup->model->collisionSpace->lock();
     psetup->model->kmodel->lock();
 

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h	2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h	2009-07-02 18:37:50 UTC (rev 18201)
@@ -68,7 +68,7 @@
 	    {
 	    }
 	    
-	    std::string getName(void);
+	    const std::string& getName(void);
 	    bool        hasParam(const std::string &param);
 	    std::string getParamString(const std::string &param, const std::string &def = "");
 	    double      getParamDouble(const std::string &param, double def);

Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp	2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp	2009-07-02 18:37:50 UTC (rev 18201)
@@ -160,7 +160,7 @@
     }
 }
 
-std::string planning_environment::RobotModels::PlannerConfig::getName(void)
+const std::string& planning_environment::RobotModels::PlannerConfig::getName(void)
 {
     return config_;
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.