Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11522] pkg/trunk
|
From: <is...@us...> - 2009-02-21 21:24:41
|
Revision: 11522
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11522&view=rev
Author: isucan
Date: 2009-02-21 21:24:37 +0000 (Sat, 21 Feb 2009)
Log Message:
-----------
better organization of requests; it should be easier to extend things now;
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPESTSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKKPIECESetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPKPIECESetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPLazyRRTSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPRRTSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPESTSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/LBKPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/LBKPIECE1.cpp
pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -44,7 +44,6 @@
#include <robot_msgs/PoseConstraint.h>
#include <ros/console.h>
-#include <boost/thread/mutex.hpp>
#include <iostream>
#include <sstream>
@@ -66,13 +65,143 @@
RKPBasicRequest(void)
{
- type = R_NONE;
+ m_type = R_NONE;
+ m_activePSetup = NULL;
}
virtual ~RKPBasicRequest(void)
{
}
+ int getType(void) const
+ {
+ return m_type;
+ }
+
+ /** Set up all the data needed by motion planning based on a request and lock the planner setup
+ * using this data */
+ bool configure(ModelMap &models, const _R &req)
+ {
+ // make sure the same motion planner instance is not used by other calls
+ m_activeReq = req;
+
+ if (!isRequestValid(models, m_activeReq))
+ return false;
+
+ ROS_INFO("Selected motion planner: '%s'", m_activeReq.params.planner_id.c_str());
+
+ /* find the data we need */
+ RKPModel *m = models[m_activeReq.params.model_id];
+
+ // lock the model
+ m->lock.lock();
+
+ // get the planner setup
+ m_activePSetup = m->planners[m_activeReq.params.planner_id];
+
+ update();
+
+ /* print some information */
+ ROS_INFO("=======================================");
+ std::stringstream ss;
+ m_activePSetup->si->printSettings(ss);
+ static_cast<StateValidityPredicate*>(m_activePSetup->si->getStateValidityChecker())->getKinematicConstraintEvaluatorSet().print(ss);
+ ROS_INFO("%s", ss.str().c_str());
+ ROS_INFO("=======================================");
+
+ /* clear memory */
+ cleanupPlanningData(m_activePSetup);
+
+ return true;
+ }
+
+ void release(void)
+ {
+ if (m_activePSetup)
+ {
+ m_activePSetup->model->lock.unlock();
+ m_activePSetup = NULL;
+ }
+ }
+
+ bool isActive(void)
+ {
+ return m_activePSetup != NULL;
+ }
+
+ RKPPlannerSetup *activePlannerSetup(void)
+ {
+ return m_activePSetup;
+ }
+
+ _R& activeRequest(void)
+ {
+ return m_activeReq;
+ }
+
+ bool isStillValid(robot_msgs::KinematicPath &path)
+ {
+ update();
+
+ /* copy the path msg into a kinematic path structure (ompl style) */
+ ompl::SpaceInformationKinematic::PathKinematic_t kpath = new ompl::SpaceInformationKinematic::PathKinematic(m_activePSetup->si);
+ unsigned int dim = m_activePSetup->si->getStateDimension();
+ for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+ {
+ ompl::SpaceInformationKinematic::StateKinematic_t kstate = new ompl::SpaceInformationKinematic::StateKinematic(dim);
+ kpath->states.push_back(kstate);
+ for (unsigned int j = 0 ; j < dim ; ++j)
+ kstate->values[j] = path.states[i].vals[j];
+ }
+
+ m_activePSetup->model->collisionSpace->lock();
+ bool valid = m_activePSetup->si->checkPath(kpath);
+ m_activePSetup->model->collisionSpace->unlock();
+
+ delete kpath;
+
+ /* clear memory */
+ cleanupPlanningData(m_activePSetup);
+
+ return valid;
+ }
+
+ bool isTrivial(double *distance = NULL)
+ {
+ update();
+
+ m_activePSetup->model->collisionSpace->lock();
+ bool trivial = m_activePSetup->mp->isTrivial(NULL, distance);
+ m_activePSetup->model->collisionSpace->unlock();
+
+ /* clear memory */
+ cleanupPlanningData(m_activePSetup);
+
+ return trivial;
+ }
+
+ void execute(robot_msgs::KinematicPath &path, double &distance, bool &trivial, bool &approximate)
+ {
+ update();
+
+ /* compute actual motion plan */
+ ompl::SpaceInformationKinematic::PathKinematic_t bestPath = NULL;
+ double bestDifference = 0.0;
+
+ m_activePSetup->model->collisionSpace->lock();
+ trivial = computePlan(m_activePSetup, m_activeReq.times, m_activeReq.allowed_time, m_activeReq.interpolate,
+ bestPath, bestDifference, approximate);
+ m_activePSetup->model->collisionSpace->unlock();
+
+ /* fill in the results */
+ fillSolution(m_activePSetup, bestPath, bestDifference, path, distance);
+
+ /* clear memory */
+ cleanupPlanningData(m_activePSetup);
+ }
+
+ protected:
+
/** Specializations of this class should implement this function */
bool isRequestValid(ModelMap &models, _R &req)
{
@@ -80,10 +209,23 @@
}
/** Specializations of this class should implement this function */
- void setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, _R &req)
+ void setupGoalState(RKPPlannerSetup *psetup, _R &req)
{
}
+ void update(void)
+ {
+ /* configure state space and starting state */
+ setupStateSpaceAndStartState(m_activePSetup, m_activeReq.params, m_activeReq.start_state);
+
+ std::vector<robot_msgs::PoseConstraint> cstrs;
+ m_activeReq.constraints.get_pose_vec(cstrs);
+ setupPoseConstraints(m_activePSetup, cstrs);
+
+ /* add goal state */
+ setupGoalState(m_activePSetup, m_activeReq);
+ }
+
/** Validate common space parameters */
bool areSpaceParamsValid(const ModelMap &modelsRef, robot_msgs::KinematicSpaceParameters ¶ms) const
{
@@ -126,7 +268,7 @@
}
/** Configure the state space for a given set of parameters and set the starting state */
- void setupStateSpaceAndStartState(RKPModel *model, RKPPlannerSetup *psetup,
+ void setupStateSpaceAndStartState(RKPPlannerSetup *psetup,
robot_msgs::KinematicSpaceParameters ¶ms,
robot_msgs::KinematicState &start_state)
{
@@ -144,15 +286,15 @@
const unsigned int dim = psetup->si->getStateDimension();
ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
- if (model->groupID >= 0)
+ if (psetup->model->groupID >= 0)
{
/* set the pose of the whole robot */
- model->kmodel->computeTransforms(&start_state.vals[0]);
- model->collisionSpace->updateRobotModel(model->collisionSpaceID);
+ psetup->model->kmodel->computeTransforms(&start_state.vals[0]);
+ psetup->model->collisionSpace->updateRobotModel(psetup->model->collisionSpaceID);
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = start_state.vals[model->kmodel->getModelInfo().groupStateIndexList[model->groupID][i]];
+ start->values[i] = start_state.vals[psetup->model->kmodel->getModelInfo().groupStateIndexList[psetup->model->groupID][i]];
}
else
{
@@ -172,7 +314,7 @@
/** Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
bool computePlan(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
- ompl::SpaceInformationKinematic::PathKinematic_t &bestPath, double &bestDifference)
+ ompl::SpaceInformationKinematic::PathKinematic_t &bestPath, double &bestDifference, bool &approximate)
{
if (times <= 0)
@@ -192,7 +334,8 @@
{
ROS_INFO("Solution already achieved");
bestDifference = t_distance;
-
+ approximate = false;
+
/* we want to maintain the invariant that a path will
at least consist of start & goal states, so we copy
the start state twice */
@@ -238,6 +381,7 @@
delete bestPath;
bestPath = path;
bestDifference = goal->getDifference();
+ approximate = goal->isApproximate();
goal->forgetSolutionPath();
ROS_INFO(" Obtained better solution: distance is %f", bestDifference);
}
@@ -281,139 +425,12 @@
psetup->si->clearGoal();
psetup->si->clearStartStates();
}
-
- bool isStillValid(ModelMap &models, _R &req, robot_msgs::KinematicPath &path)
- {
- // make sure the same motion planner instance is not used by other calls
- boost::mutex::scoped_lock(m_lock);
-
- if (!isRequestValid(models, req))
- return false;
-
- /* find the data we need */
- RKPModel *m = models[req.params.model_id];
- RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
-
- /* configure state space and starting state */
- setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
- std::vector<robot_msgs::PoseConstraint> cstrs;
- req.constraints.get_pose_vec(cstrs);
- setupPoseConstraints(psetup, cstrs);
-
- /* add goal state */
- setupGoalState(m, psetup, req);
-
- /* copy the path msg into a kinematic path structure (ompl style) */
- ompl::SpaceInformationKinematic::PathKinematic_t kpath = new ompl::SpaceInformationKinematic::PathKinematic(psetup->si);
- unsigned int dim = psetup->si->getStateDimension();
- for (unsigned int i = 0 ; i < path.states.size() ; ++i)
- {
- ompl::SpaceInformationKinematic::StateKinematic_t kstate = new ompl::SpaceInformationKinematic::StateKinematic(dim);
- kpath->states.push_back(kstate);
- for (unsigned int j = 0 ; j < dim ; ++j)
- kstate->values[j] = path.states[i].vals[j];
- }
-
- m->collisionSpace->lock();
- bool valid = psetup->si->checkPath(kpath);
- m->collisionSpace->unlock();
-
- delete kpath;
-
- /* clear memory */
- cleanupPlanningData(psetup);
-
- return valid;
- }
-
- bool isTrivial(ModelMap &models, _R &req, double *distance = NULL)
- {
- // make sure the same motion planner instance is not used by other calls
- boost::mutex::scoped_lock(m_lock);
-
- if (!isRequestValid(models, req))
- return false;
-
- /* find the data we need */
- RKPModel *m = models[req.params.model_id];
- RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
-
- /* configure state space and starting state */
- setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
-
- std::vector<robot_msgs::PoseConstraint> cstrs;
- req.constraints.get_pose_vec(cstrs);
- setupPoseConstraints(psetup, cstrs);
-
- /* add goal state */
- setupGoalState(m, psetup, req);
-
- m->collisionSpace->lock();
- bool trivial = psetup->mp->isTrivial(NULL, distance);
- m->collisionSpace->unlock();
-
- /* clear memory */
- cleanupPlanningData(psetup);
-
- return trivial;
- }
-
- bool execute(ModelMap &models, _R &req, robot_msgs::KinematicPath &path, double &distance, bool &trivial)
- {
- // make sure the same motion planner instance is not used by other calls
- boost::mutex::scoped_lock(m_lock);
-
- if (!isRequestValid(models, req))
- return false;
-
- ROS_INFO("Selected motion planner: '%s'", req.params.planner_id.c_str());
-
- /* find the data we need */
- RKPModel *m = models[req.params.model_id];
- RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
-
- /* configure state space and starting state */
- setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
-
- std::vector<robot_msgs::PoseConstraint> cstrs;
- req.constraints.get_pose_vec(cstrs);
- setupPoseConstraints(psetup, cstrs);
-
- /* add goal state */
- setupGoalState(m, psetup, req);
-
- /* print some information */
- ROS_INFO("=======================================");
- std::stringstream ss;
- psetup->si->printSettings(ss);
- static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->getKinematicConstraintEvaluatorSet().print(ss);
- ROS_INFO("%s", ss.str().c_str());
- ROS_INFO("=======================================");
-
- /* compute actual motion plan */
- ompl::SpaceInformationKinematic::PathKinematic_t bestPath = NULL;
- double bestDifference = 0.0;
-
- m->collisionSpace->lock();
- trivial = computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
- m->collisionSpace->unlock();
-
- /* fill in the results */
- fillSolution(psetup, bestPath, bestDifference, path, distance);
-
- /* clear memory */
- cleanupPlanningData(psetup);
-
- return true;
- }
-
// the type of request (unique ID for each type)
- int type;
+ int m_type;
- protected:
-
- boost::mutex m_lock;
+ _R m_activeReq;
+ RKPPlannerSetup *m_activePSetup;
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -50,7 +50,7 @@
{
public:
- GoalToPosition(ompl::SpaceInformation_t si, RKPModel *model, const std::vector<robot_msgs::PoseConstraint> &pc) : ompl::SpaceInformationKinematic::GoalRegionKinematic(si)
+ GoalToPosition(ompl::SpaceInformation_t si, RKPModelBase *model, const std::vector<robot_msgs::PoseConstraint> &pc) : ompl::SpaceInformationKinematic::GoalRegionKinematic(si)
{
m_model = model;
for (unsigned int i = 0 ; i < pc.size() ; ++i)
@@ -129,7 +129,7 @@
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
}
- mutable RKPModel *m_model;
+ mutable RKPModelBase *m_model;
std::vector<PoseConstraintEvaluator*> m_pce;
};
@@ -137,7 +137,7 @@
template<>
RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest>::RKPBasicRequest(void)
{
- type = R_POSITION;
+ m_type = R_POSITION;
}
/** Validate request for planning towards a link position */
@@ -160,13 +160,13 @@
/** Set the goal using a destination link position */
template<>
- void RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest>::setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, robot_msgs::KinematicPlanLinkPositionRequest &req)
+ void RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest>::setupGoalState(RKPPlannerSetup *psetup, robot_msgs::KinematicPlanLinkPositionRequest &req)
{
/* set the goal */
std::vector<robot_msgs::PoseConstraint> pc;
req.get_goal_constraints_vec(pc);
- GoalToPosition *goal = new GoalToPosition(psetup->si, model, pc);
+ GoalToPosition *goal = new GoalToPosition(psetup->si, psetup->model, pc);
psetup->si->setGoal(goal);
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -49,7 +49,7 @@
template<>
RKPBasicRequest<robot_msgs::KinematicPlanStateRequest>::RKPBasicRequest(void)
{
- type = R_STATE;
+ m_type = R_STATE;
}
/** Validate request for planning towards a state */
@@ -79,7 +79,7 @@
/** Set the goal using a destination state */
template<>
- void RKPBasicRequest<robot_msgs::KinematicPlanStateRequest>::setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, robot_msgs::KinematicPlanStateRequest &req)
+ void RKPBasicRequest<robot_msgs::KinematicPlanStateRequest>::setupGoalState(RKPPlannerSetup *psetup, robot_msgs::KinematicPlanStateRequest &req)
{
/* set the goal */
ompl::SpaceInformationKinematic::GoalStateKinematic_t goal = new ompl::SpaceInformationKinematic::GoalStateKinematic(psetup->si);
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -67,65 +67,65 @@
delete i->second;
}
- void addRRT(std::map<std::string, std::string> &options)
+ void addRRT(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *rrt = new RKPRRTSetup();
- if (rrt->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *rrt = new RKPRRTSetup(dynamic_cast<RKPModelBase*>(this));
+ if (rrt->setup(options))
planners[rrt->name] = rrt;
else
delete rrt;
}
- void addLazyRRT(std::map<std::string, std::string> &options)
+ void addLazyRRT(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *rrt = new RKPLazyRRTSetup();
- if (rrt->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *rrt = new RKPLazyRRTSetup(dynamic_cast<RKPModelBase*>(this));
+ if (rrt->setup(options))
planners[rrt->name] = rrt;
else
delete rrt;
}
- void addEST(std::map<std::string, std::string> &options)
+ void addEST(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *est = new RKPESTSetup();
- if (est->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *est = new RKPESTSetup(dynamic_cast<RKPModelBase*>(this));
+ if (est->setup(options))
planners[est->name] = est;
else
delete est;
}
- void addSBL(std::map<std::string, std::string> &options)
+ void addSBL(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *sbl = new RKPSBLSetup();
- if (sbl->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *sbl = new RKPSBLSetup(dynamic_cast<RKPModelBase*>(this));
+ if (sbl->setup(options))
planners[sbl->name] = sbl;
else
delete sbl;
}
- void addIKSBL(std::map<std::string, std::string> &options)
+ void addIKSBL(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *sbl = new RKPIKSBLSetup();
- if (sbl->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *sbl = new RKPIKSBLSetup(dynamic_cast<RKPModelBase*>(this));
+ if (sbl->setup(options))
planners[sbl->name] = sbl;
else
delete sbl;
}
- void addKPIECE(std::map<std::string, std::string> &options)
+ void addKPIECE(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *kpiece = new RKPKPIECESetup();
- if (kpiece->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *kpiece = new RKPKPIECESetup(dynamic_cast<RKPModelBase*>(this));
+ if (kpiece->setup(options))
planners[kpiece->name] = kpiece;
else
delete kpiece;
}
- void addIKKPIECE(std::map<std::string, std::string> &options)
+ void addIKKPIECE(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *kpiece = new RKPIKKPIECESetup();
- if (kpiece->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *kpiece = new RKPIKKPIECESetup(dynamic_cast<RKPModelBase*>(this));
+ if (kpiece->setup(options))
planners[kpiece->name] = kpiece;
else
delete kpiece;
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPESTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPESTSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPESTSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPESTSetup(void);
+ RKPESTSetup(RKPModelBase *m);
virtual ~RKPESTSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKKPIECESetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKKPIECESetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKKPIECESetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPIKKPIECESetup(void);
+ RKPIKKPIECESetup(RKPModelBase *m);
virtual ~RKPIKKPIECESetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKSBLSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKSBLSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPIKSBLSetup(void);
+ RKPIKSBLSetup(RKPModelBase *m);
virtual ~RKPIKSBLSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPKPIECESetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPKPIECESetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPKPIECESetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPKPIECESetup(void);
+ RKPKPIECESetup(RKPModelBase *m);
virtual ~RKPKPIECESetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPLazyRRTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPLazyRRTSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPLazyRRTSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPLazyRRTSetup(void);
+ RKPLazyRRTSetup(RKPModelBase *m);
virtual ~RKPLazyRRTSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -58,22 +58,23 @@
{
public:
- RKPPlannerSetup(void);
+ RKPPlannerSetup(RKPModelBase *m);
virtual ~RKPPlannerSetup(void);
/** For each planner definition, define the set of distance metrics it can use */
virtual void setupDistanceEvaluators(void);
- virtual ompl::ProjectionEvaluator* getProjectionEvaluator(RKPModelBase *model,
- const std::map<std::string, std::string> &options) const;
+ virtual ompl::ProjectionEvaluator* getProjectionEvaluator(const std::map<std::string, std::string> &options) const;
- virtual void preSetup(RKPModelBase *model, std::map<std::string, std::string> &options);
- virtual void postSetup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual void preSetup(const std::map<std::string, std::string> &options);
+ virtual void postSetup(const std::map<std::string, std::string> &options);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options) = 0;
+ virtual bool setup(const std::map<std::string, std::string> &options) = 0;
- std::string name;
+ RKPModelBase *model;
+
+ std::string name;
ompl::Planner *mp;
ompl::GAIK *gaik;
ompl::SpaceInformationKinematic *si;
@@ -83,7 +84,10 @@
protected:
- double parseDouble(const std::string &value, double def);
+ double parseDouble(const std::string &value, double def) const;
+ bool hasOption(const std::map<std::string, std::string> &options, const std::string &opt) const;
+ double optionAsDouble(const std::map<std::string, std::string> &options, const std::string &opt, double def) const;
+ std::string optionAsString(const std::map<std::string, std::string> &options, const std::string &opt, const::std::string &def) const;
};
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPRRTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPRRTSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPRRTSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPRRTSetup(void);
+ RKPRRTSetup(RKPModelBase *m);
virtual ~RKPRRTSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPSBLSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPSBLSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPSBLSetup(void);
+ RKPSBLSetup(RKPModelBase *m);
virtual ~RKPSBLSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPESTSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPESTSetup.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPESTSetup.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -37,7 +37,7 @@
#include "kinematic_planning/ompl_planner/RKPESTSetup.h"
-kinematic_planning::RKPESTSetup::RKPESTSetup(void) : RKPPlannerSetup()
+kinematic_planning::RKPESTSetup::RKPESTSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "EST";
}
@@ -52,22 +52,27 @@
}
}
-bool kinematic_planning::RKPESTSetup::setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+bool kinematic_planning::RKPESTSetup::setup(const std::map<std::string, std::string> &options)
{
- preSetup(model, options);
+ preSetup(options);
ompl::EST *est = new ompl::EST(si);
mp = est;
-
- if (options.find("range") != options.end())
+
+ if (hasOption(options, "range"))
{
- double range = parseDouble(options["range"], est->getRange());
- est->setRange(range);
- ROS_INFO("Range is set to %g", range);
+ est->setRange(optionAsDouble(options, "range", est->getRange()));
+ ROS_INFO("Range is set to %g", est->getRange());
}
+
+ if (hasOption(options, "goal_bias"))
+ {
+ est->setGoalBias(optionAsDouble(options, "goal_bias", est->getGoalBias()));
+ ROS_INFO("Goal bias is set to %g", est->getGoalBias());
+ }
+
+ est->setProjectionEvaluator(getProjectionEvaluator(options));
- est->setProjectionEvaluator(getProjectionEvaluator(model, options));
-
if (est->getProjectionEvaluator() == NULL)
{
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
@@ -75,7 +80,7 @@
}
else
{
- postSetup(model, options);
+ postSetup(options);
return true;
}
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -36,7 +36,7 @@
#include "kinematic_planning/ompl_planner/RKPIKKPIECESetup.h"
-kinematic_planning::RKPIKKPIECESetup::RKPIKKPIECESetup(void) : RKPPlannerSetup()
+kinematic_planning::RKPIKKPIECESetup::RKPIKKPIECESetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "IKKPIECE";
}
@@ -51,22 +51,34 @@
}
}
-bool kinematic_planning::RKPIKKPIECESetup::setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+bool kinematic_planning::RKPIKKPIECESetup::setup(const std::map<std::string, std::string> &options)
{
- preSetup(model, options);
+ preSetup(options);
ompl::IKKPIECE1* kpiece = new ompl::IKKPIECE1(si);
mp = kpiece;
+
- if (options.find("range") != options.end())
+ if (hasOption(options, "range"))
{
- double range = parseDouble(options["range"], kpiece->getRange());
- kpiece->setRange(range);
- ROS_INFO("Range is set to %g", range);
+ kpiece->setRange(optionAsDouble(options, "range", kpiece->getRange()));
+ ROS_INFO("Range is set to %g", kpiece->getRange());
}
- kpiece->setProjectionEvaluator(getProjectionEvaluator(model, options));
+ if (hasOption(options, "ik_range"))
+ {
+ kpiece->setIKRange(optionAsDouble(options, "ik_range", kpiece->getIKRange()));
+ ROS_INFO("IK range is set to %g", kpiece->getIKRange());
+ }
+ if (hasOption(options, "goal_bias"))
+ {
+ kpiece->setGoalBias(optionAsDouble(options, "goal_bias", kpiece->getGoalBias()));
+ ROS_INFO("Goal bias is set to %g", kpiece->getGoalBias());
+ }
+
+ kpiece->setProjectionEvaluator(getProjectionEvaluator(options));
+
if (kpiece->getProjectionEvaluator() == NULL)
{
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
@@ -74,7 +86,7 @@
}
else
{
- postSetup(model, options);
+ postSetup(options);
return true;
}
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -36,7 +36,7 @@
#include "kinematic_planning/ompl_planner/RKPIKSBLSetup.h"
-kinematic_planning::RKPIKSBLSetup::RKPIKSBLSetup(void) : RKPPlannerSetup()
+kinematic_planning::RKPIKSBLSetup::RKPIKSBLSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "IKSBL";
}
@@ -51,21 +51,26 @@
}
}
-bool kinematic_planning::RKPIKSBLSetup::setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+bool kinematic_planning::RKPIKSBLSetup::setup(const std::map<std::string, std::string> &options)
{
- preSetup(model, options);
+ preSetup(options);
ompl::IKSBL* sbl = new ompl::IKSBL(si);
mp = sbl;
+
+ if (hasOption(options, "range"))
+ {
+ sbl->setRange(optionAsDouble(options, "range", sbl->getRange()));
+ ROS_INFO("Range is set to %g", sbl->getRange());
+ }
- if (options.find("range") != options.end())
+ if (hasOption(options, "ik_range"))
{
- double range = parseDouble(options["range"], sbl->getRange());
- sbl->setRange(range);
- ROS_INFO("Range is set to %g", range);
+ sbl->setIKRange(optionAsDouble(options, "ik_range", sbl->getIKRange()));
+ ROS_INFO("IK range is set to %g", sbl->getIKRange());
}
- sbl->setProjectionEvaluator(getProjectionEvaluator(model, options));
+ sbl->setProjectionEvaluator(getProjectionEvaluator(options));
if (sbl->getProjectionEvaluator() == NULL)
{
@@ -74,7 +79,7 @@
}
else
{
- postSetup(model, options);
+ postSetup(options);
return true;
}
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -36,7 +36,7 @@
#include "kinematic_planning/ompl_planner/RKPKPIECESetup.h"
-kinematic_planning::RKPKPIECESetup::RKPKPIECESetup(void) : RKPPlannerSetup()
+kinematic_planning::RKPKPIECESetup::RKPKPIECESetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "KPIECE";
}
@@ -51,22 +51,27 @@
}
}
-bool kinematic_planning::RKPKPIECESetup::setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+bool kinematic_planning::RKPKPIECESetup::setup(const std::map<std::string, std::string> &options)
{
- preSetup(model, options);
+ preSetup(options);
ompl::KPIECE1 *kpiece = new ompl::KPIECE1(si);
mp = kpiece;
- if (options.find("range") != options.end())
+ if (hasOption(options, "range"))
{
- double range = parseDouble(options["range"], kpiece->getRange());
- kpiece->setRange(range);
- ROS_INFO("Range is set to %g", range);
+ kpiece->setRange(optionAsDouble(options, "range", kpiece->getRange()));
+ ROS_INFO("Range is set to %g", kpiece->getRange());
}
- kpiece->setProjectionEvaluator(getProjectionEvaluator(model, options));
+ if (hasOption(options, "goal_bias"))
+ {
+ kpiece->setGoalBias(optionAsDouble(options, "goal_bias", kpiece->getGoalBias()));
+ ROS_INFO("Goal bias is set to %g", kpiece->getGoalBias());
+ }
+ kpiece->setProjectionEvaluator(getProjectionEvaluator(options));
+
if (kpiece->getProjectionEvaluator() == NULL)
{
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
@@ -74,7 +79,7 @@
}
else
{
- postSetup(model, options);
+ postSetup(options);
return true;
}
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -37,7 +37,7 @@
#include "kinematic_planning/ompl_planner/RKPLazyRRTSetup.h"
#include <ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h>
-kinematic_planning::RKPLazyRRTSetup::RKPLazyRRTSetup(void) : RKPPlannerSetup()
+kinematic_planning::RKPLazyRRTSetup::RKPLazyRRTSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "LazyRRT";
}
@@ -46,28 +46,26 @@
{
}
-bool kinematic_planning::RKPLazyRRTSetup::setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+bool kinematic_planning::RKPLazyRRTSetup::setup(const std::map<std::string, std::string> &options)
{
- preSetup(model, options);
+ preSetup(options);
ompl::LazyRRT *rrt = new ompl::LazyRRT(si);
- mp = rrt;
-
- if (options.find("range") != options.end())
+ mp = rrt;
+
+ if (hasOption(options, "range"))
{
- double range = parseDouble(options["range"], rrt->getRange());
- rrt->setRange(range);
- ROS_INFO("Range is set to %g", range);
+ rrt->setRange(optionAsDouble(options, "range", rrt->getRange()));
+ ROS_INFO("Range is set to %g", rrt->getRange());
}
- if (options.find("goal_bias") != options.end())
- {
- double bias = parseDouble(options["goal_bias"], rrt->getGoalBias());
- rrt->setGoalBias(bias);
- ROS_INFO("Goal bias is set to %g", bias);
+ if (hasOption(options, "goal_bias"))
+ {
+ rrt->setGoalBias(optionAsDouble(options, "goal_bias", rrt->getGoalBias()));
+ ROS_INFO("Goal bias is set to %g", rrt->getGoalBias());
}
- postSetup(model, options);
+ postSetup(options);
return true;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -42,8 +42,9 @@
#include <cassert>
#include <vector>
-kinematic_planning::RKPPlannerSetup::RKPPlannerSetup(void)
+kinematic_planning::RKPPlannerSetup::RKPPlannerSetup(RKPModelBase *m)
{
+ model = m;
mp = NULL;
gaik = NULL;
si = NULL;
@@ -75,7 +76,7 @@
sde["L2Square"] = new ompl::SpaceInformationKinematic::StateKinematicL2SquareDistanceEvaluator(si);
}
-ompl::ProjectionEvaluator* kinematic_planning::RKPPlannerSetup::getProjectionEvaluator(RKPModelBase *model, const std::map<std::string, std::string> &options) const
+ompl::ProjectionEvaluator* kinematic_planning::RKPPlannerSetup::getProjectionEvaluator(const std::map<std::string, std::string> &options) const
{
std::map<std::string, std::string>::const_iterator pit = options.find("projection");
std::map<std::string, std::string>::const_iterator cit = options.find("celldim");
@@ -124,7 +125,7 @@
return pe;
}
-void kinematic_planning::RKPPlannerSetup::preSetup(RKPModelBase *model, std::map<std::string, std::string> &options)
+void kinematic_planning::RKPPlannerSetup::preSetup(const std::map<std::string, std::string> &options)
{
ROS_INFO("Adding %s instance for motion planning: %s", name.c_str(), model->groupName.c_str());
@@ -139,15 +140,32 @@
gaik = new ompl::GAIK(si);
}
-void kinematic_planning::RKPPlannerSetup::postSetup(RKPModelBase *model, std::map<std::string, std::string> &options)
+void kinematic_planning::RKPPlannerSetup::postSetup(const std::map<std::string, std::string> &options)
{
setupDistanceEvaluators();
si->setup();
mp->setup();
}
-double kinematic_planning::RKPPlannerSetup::parseDouble(const std::string &value, double def)
+bool kinematic_planning::RKPPlannerSetup::hasOption(const std::map<std::string, std::string> &options, const std::string &opt) const
{
+ return options.find(opt) != options.end();
+}
+
+double kinematic_planning::RKPPlannerSetup::optionAsDouble(const std::map<std::string, std::string> &options, const std::string &opt, double def) const
+{
+ std::map<std::string, std::string>::const_iterator it = options.find(opt);
+ return (it != options.end()) ? parseDouble(it->second, def) : def;
+}
+
+std::string kinematic_planning::RKPPlannerSetup::optionAsString(const std::map<std::string, std::string> &options, const std::string &opt, const::std::string &def) const
+{
+ std::map<std::string, std::string>::const_iterator it = options.find(opt);
+ return (it != options.end()) ? it->second : def;
+}
+
+double kinematic_planning::RKPPlannerSetup::parseDouble(const std::string &value, double def) const
+{
try
{
return boost::lexical_cast<double>(value);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -36,7 +36,7 @@
#include "kinematic_planning/ompl_planner/RKPRRTSetup.h"
-kinematic_planning::RKPRRTSetup::RKPRRTSetup(void) : RKPPlannerSetup()
+kinematic_planning::RKPRRTSetup::RKPRRTSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "RRT";
}
@@ -45,29 +45,27 @@
{
}
-bool kinematic_planning::RKPRRTSetup::setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+bool kinematic_planning::RKPRRTSetup::setup(const std::map<std::string, std::string> &options)
{
- preSetup(model, options);
+ preSetup(options);
ompl::RRT *rrt = new ompl::RRT(si);
mp = rrt;
- if (options.find("range") != options.end())
+ if (hasOption(options, "range"))
{
- double range = parseDouble(options["range"], rrt->getRange());
- rrt->setRange(range);
- ROS_INFO("Range is set to %g", range);
+ rrt->setRange(optionAsDouble(options, "range", rrt->getRange()));
+ ROS_INFO("Range is set to %g", rrt->getRange());
}
- if (options.find("goal_bias") != options.end())
- {
- double bias = parseDouble(options["goal_bias"], rrt->getGoalBias());
- rrt->setGoalBias(bias);
- ROS_INFO("Goal bias is set to %g", bias);
+ if (hasOption(options, "goal_bias"))
+ {
+ rrt->setGoalBias(optionAsDouble(options, "goal_bias", rrt->getGoalBias()));
+ ROS_INFO("Goal bias is set to %g", rrt->getGoalBias());
}
+
+ postSetup(options);
- postSetup(model, options);
-
return true;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -36,7 +36,7 @@
#include "kinematic_planning/ompl_planner/RKPSBLSetup.h"
-kinematic_planning::RKPSBLSetup::RKPSBLSetup(void) : RKPPlannerSetup()
+kinematic_planning::RKPSBLSetup::RKPSBLSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "SBL";
}
@@ -51,22 +51,21 @@
}
}
-bool kinematic_planning::RKPSBLSetup::setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+bool kinematic_planning::RKPSBLSetup::setup(const std::map<std::string, std::string> &options)
{
- preSetup(model, options);
+ preSetup(options);
ompl::SBL *sbl = new ompl::SBL(si);
mp = sbl;
- if (options.find("range") != options.end())
+ if (hasOption(options, "range"))
{
- double range = parseDouble(options["range"], sbl->getRange());
- sbl->setRange(range);
- ROS_INFO("Range is set to %g", range);
+ sbl->setRange(optionAsDouble(options, "range", sbl->getRange()));
+ ROS_INFO("Range is set to %g", sbl->getRange());
}
+
+ sbl->setProjectionEvaluator(getProjectionEvaluator(options));
- sbl->setProjectionEvaluator(getProjectionEvaluator(model, options));
-
if (sbl->getProjectionEvaluator() == NULL)
{
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
@@ -74,7 +73,7 @@
}
else
{
- postSetup(model, options);
+ postSetup(options);
return true;
}
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-21 21:24:37 UTC (rev 11522)
@@ -272,6 +272,11 @@
m_continueReplanningLock.lock();
if (m_currentRequestType != R_NONE)
{
+ if (m_currentRequestType == R_STATE)
+ m_requestState.release();
+ else
+ m_requestLinkPosition.release();
+
/* make sure the working thread knows it is time to stop */
m_currentRequestType = R_NONE;
m_collisionMonitorCondition.notify_all();
@@ -288,7 +293,9 @@
}
m_replanningLock.unlock();
- ROS_INFO("Replanning stopped");
+
+ if (stop)
+ ROS_INFO("Replanning stopped");
}
bool stopReplanning(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
@@ -316,6 +323,47 @@
delete m_statusThread;
}
}
+
+ bool replanToState(robot_srvs::KinematicReplanState::Request &req, robot_srvs::KinematicReplanState::Response &res)
+ {
+ ROS_INFO("Request for replanning to a state");
+ bool st = false;
+ res.id = -1;
+
+ stopReplanning();
+
+ if (m_robotState)
+ {
+ currentState(req.value.start_state);
+ st = m_requestState.configure(m_models, req.value);
+
+ if (st)
+ {
+ // start planning thread
+ m_replanningLock.lock();
+ m_currentRequestType = R_STATE;
+
+ m_currentPlanStatus.id = ++m_replanID;
+ m_currentPlanStatus.valid = 1;
+ m_currentPlanStatus.path.set_states_size(0);
+ m_currentPlanStatus.done = 0;
+ m_currentPlanStatus.approximate = 1;
+ m_currentPlanStatus.distance = -1.0;
+ res.id = m_currentPlanStatus.id;
+ m_statusLock.unlock();
+
+ ROS_INFO("Start replanning with plan id %d", res.id);
+ m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToStateThread, this));
+ m_replanningLock.unlock();
+ }
+ else
+ ROS_ERROR("Received invalid request");
+ }
+ else
+ ROS_ERROR("Current robot state is unknown. Cannot start replanning.");
+
+ return st;
+ }
bool replanToPosition(robot_srvs::KinematicReplanLinkPosition::Request &req, robot_srvs::KinematicReplanLinkPosition::Response &res)
{
@@ -327,26 +375,30 @@
if (m_robotState)
{
- // back up the request
- m_currentPlanToPositionRequest = req.value;
-
- // start planning thread
- m_replanningLock.lock();
- m_currentRequestType = R_POSITION;
+ currentState(req.value.start_state);
+ st = m_requestLinkPosition.configure(m_models, req.value);
- m_currentPlanStatus.id = ++m_replanID;
- m_currentPlanStatus.valid = 1;
- m_currentPlanStatus.path.set_states_size(0);
- m_currentPlanStatus.done = 0;
- m_currentPlanStatus.distance = -1.0;
- res.id = m_currentPlanStatus.id;
- m_statusLock.unlock();
-
- ROS_INFO("Start replanning with plan id %d", res.id);
- m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToPositionThread, this));
- m_replanningLock.unlock();
-
- st = true;
+ if (st)
+ {
+ // start planning thread
+ m_replanningLock.lock();
+ m_currentRequestType = R_POSITION;
+
+ m_currentPlanStatus.id = ++m_replanID;
+ m_currentPlanStatus.valid = 1;
+ m_currentPlanStatus.path.set_states_size(0);
+ m_currentPlanStatus.done = 0;
+ m_currentPlanStatus.approximate = 1;
+ m_currentPlanStatus.distance = -1.0;
+ res.id = m_currentPlanStatus.id;
+ m_statusLock.unlock();
+
+ ROS_INFO("Start replanning with plan id %d", res.id);
+ m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToPositionThread, this));
+ m_replanningLock.unlock();
+ }
+ else
+ ROS_ERROR("Received invalid request");
}
else
ROS_ERROR("Current robot state is unknown. Cannot start replanning.");
@@ -358,19 +410,26 @@
{
ROS_INFO("Request for planning to a state");
bool trivial = false;
+ bool approximate = false;
+
if (req.value.start_state.get_vals_size() == 0)
{
currentState(req.value.start_state);
ROS_INFO("Using current state as starting point");
}
- bool result = false;
+ res.value.unsafe = isSafeToPlan(true) ? 0 : 1;
+ bool result = m_requestStateOpenLoop.configure(m_models, req.value);
+ if (result)
+ {
+ m_requestStateOpenLoop.execute(res.value.path, res.value.distance, trivial, approximate);
+ m_requestStateOpenLoop.release();
+ }
- res.value.unsafe = isSafeToPlan(true) ? 0 : 1;
- result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
res.value.id = -1;
res.value.done = trivial ? 1 : 0;
res.value.valid = res.value.path.get_states_size() > 0;
+ res.value.approximate = approximate ? 1 : 0;
return result;
}
@@ -379,21 +438,27 @@
{
ROS_INFO("Request for planning to a position");
bool trivial = false;
+ bool approximate = false;
+
if (req.value.start_state.get_vals_size() == 0)
{
currentState(req.value.start_state);
ROS_INFO("Using current state as starting point");
}
- bool result = false;
-
res.value.unsafe = isSafeToPlan(true) ? 0 : 1;
- result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
-
+ bool result = m_requestLinkPositionOpenLoop.configure(m_models, req.value);
+ if (result)
+ {
+ m_requestLinkPositionOpenLoop.execute(res.value.path, res.value.distance, trivial, approximate);
+ m_requestLinkPositionOpenLoop.release();
+ }
+
res.value.id = -1;
res.value.done = trivial ? 1 : 0;
res.value.valid = res.value.path.get_states_size() > 0;
-
+ res.value.approximate = approximate ? 1 : 0;
+
return result;
}
@@ -485,15 +550,15 @@
// check if we reached the goal position
if (m_currentRequestType == R_STATE)
{
- currentState(m_currentPlanToStateRequest.start_state);
- m_currentPlanStatus.done = m_requestState.isTrivial(m_models, m_currentPlanToStateRequest, &m_currentPlanStatus.distance) ? 1 : 0;
+ currentState(m_requestState.activeRequest().start_state);
+ m_currentPlanStatus.done = m_requestState.isTrivial(&m_currentPlanStatus.distance) ? 1 : 0;
issueStop = m_currentPlanStatus.done;
}
else
if (m_currentRequestType == R_POSITION)
{
- currentState(m_currentPlanToPositionRequest.start_state);
- m_currentPlanStatus.done = m_requestLinkPosition.isTrivial(m_models, m_currentPlanToPositionRequest, &m_currentPlanStatus.distance) ? 1 : 0;
+ currentState(m_requestLinkPosition.activeRequest().start_state);
+ m_currentPlanStatus.done = m_requestLinkPosition.isTrivial(&m_currentPlanStatus.distance) ? 1 : 0;
issueStop = m_currentPlanStatus.done;
}
}
@@ -536,43 +601,7 @@
}
}
}
-
- bool replanToState(robot_srvs::KinematicReplanState::Request &req, robot_srvs::KinematicReplanState::Response &res)
- {
- ROS_INFO("Request for replanning to a state");
- bool st = false;
- res.id = -1;
-
- stopReplanning();
-
- if (m_robotState)
- {
- // back up the request
- m_currentPlanToStateRequest = req.value;
-
- // start planning thread
- m_replanningLock.lock();
- m_currentRequestType = R_STATE;
-
- m_currentPlanStatus.id = ++m_replanID;
- m_currentPlanStatus.valid = 1;
- m_currentPlanStatus.path.set_states_size(0);
- m_currentPlanStatus.done = 0;
- m_currentPlanStatus.distance = -1.0;
- res.id = m_currentPlanStatus.id;
- m_statusLock.unlock();
-
- ROS_INFO("Start replanning with plan id %d", res.id);
- m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToStateThread, this));
- m_replanningLock.unlock();
- st = true;
- }
- else
- ROS_ERROR("Current robot state is unknown. Cannot start replanning.");
-
- return st;
- }
-
+
/** Wait for a change in the environment and recompute the motion plan */
void replanToStateThread(void)
{
@@ -580,6 +609,8 @@
robot_msgs::KinematicPath solution;
unsigned int step = 0;
bool trivial = false;
+ bool approximate = false;
+
while (m_currentRequestType == R_STATE && !trivial)
{
step++;
@@ -588,16 +619,17 @@
m_collisionMonitorChange = false;
double distance = 0.0;
bool safe = isSafeToPlan(true);
-
- currentState(m_currentPlanToStateRequest.start_state);
- m_currentlyExecutedPathStart = m_currentPlanToStateRequest.start_state;
- m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance, trivial);
+
+ currentState(m_requestState.activeRequest().start_state);
+ m_currentlyExecutedPathStart = m_requestState.activeRequest().start_state;
+ m_requestState.execute(solution, distance, trivial, approximate);
bool foundSolution = solution.get_states_size() > 0;
m_statusLock.lock();
m_currentPlanStatus.path = solution;
m_currentPlanStatus.distance = distance;
m_currentPlanStatus.done = trivial ? 1 : 0;
+ m_currentPlanStatus.approximate = approximate ? 1 : 0;
m_currentPlanStatus.valid = foundSolution ? 1 : 0;
m_currentPlanStatus.unsafe = safe ? 0 : 1;
m_statusLock.unlock();
@@ -626,6 +658,7 @@
robot_msgs::KinematicPath solution;
unsigned int step = 0;
bool trivial = false;
+ bool approximate = false;
while (m_currentRequestType == R_POSITION && !trivial)
{
@@ -636,15 +669,16 @@
double distance = 0.0;
bool safe = isSafeToPlan(true);
- currentState(m_currentPlanToPositionRequest.start_state);
- m_currentlyExecutedPathStart = m_currentPlanToPositionRequest.start_state;
- m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance, trivial);
+ currentState(m_requestLinkPosition.activeRequest().start_state);
+ m_currentlyExecutedPathStart = m_requestLinkPosition.activeRequest().start_state;
+ m_requestLinkPosition.execute(solution, distance, trivial, approximate);
bool foundSolution = solution.get_states_size() > 0;
m_statusLock.lock();
m_currentPlanStatus.path = solution;
m_currentPlanStatus.distance = distance;
m_currentPlanStatus.done = trivial ? 1 : 0;
+ m_currentPlanStatus.approximate = approximate ? 1 : 0;
m_currentPlanStatus.valid = foundSolution ? 1 : 0;
m_currentPlanStatus.unsafe = safe ? 0 : 1;
m_statusLock.unlock();
@@ -678,14 +712,14 @@
{
if (m_currentRequestType == R_STATE)
{
- m_currentPlanToStateRequest.start_state = m_currentlyExecutedPathStart;
- update = !m_requestState.isStillValid(m_models, m_currentPlanToStateRequest, m_currentlyExecutedPath);
+ m_requestState.activeRequest().start_state = m_currentlyExecutedPathStart;
+ update = !m_requestState.isStillValid(m_currentlyExecutedPath);
}
else
if (m_currentRequestType == R_POSITION)
{
- m_currentPlanToPositionRequest.start_state = m_currentlyExecutedPathStart;
- update = !m_requestLinkPosition.isStillValid(m_models, m_currentPlanToPositionRequest, m_currentlyExecutedPath);
+ m_requestLinkPosition.activeRequest().start_state = m_currentlyExecutedPathStart;
+ update = !m_requestLinkPosition.isStillValid(m_currentlyExecutedPath);
}
if (update)
@@ -780,6 +814,8 @@
}
ModelMap m_models;
+ RKPBasicRequest<robot_msgs::KinematicPlanStateRequest> m_requestStateOpenLoop;
+ RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest> m_requestLinkPositionOpenLoop;
RKPBasicRequest<robot_msgs::KinematicPlanStateRequest> m_requestState;
RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest> m_requestLinkPosition;
@@ -793,8 +829,6 @@
/*********** DATA USED FOR REPLANNING ONLY ***********/
// currently considered request
- robot_msgs::KinematicPlanStateRequest m_currentPlanToStateRequest;
- robot_msgs::KinematicPlanLinkPositionRequest m_currentPlanToPositi...
[truncated message content] |
