Commit 663cc860 authored by Alireza Nakhaei's avatar Alireza Nakhaei
Browse files

Enable the users to replace the list of obstacles in the problem (Remove and Add)

After the modifications, ChppProblem::obstacleList (inCollisionList) will replace list of obstacles. Before the modification, it just added the elements of the new list to the existing list. so, the following modification were done:
1- Add an attribute ,”attMapOuter”, to hppProblem
2- Add  a function “initMapOuter()” to hppProblem
3– Modify the constructors and also  obstacleList (inCollisionList) in hppProblem
parent 53c2012d
......@@ -27,204 +27,217 @@ INCLUDE
/*************************************
CLASS
**************************************/
/**
\brief Defines a path planning problem for one robot.
A path planning problem is defined by
\li a robot: a KineoWorks device,
\li a set of obstacles: a list of Kcd objects,
\li a steering method: stored in the KwsDevice,
/**
\brief Defines a path planning problem for one robot.
A path planning problem is defined by
\li a robot: a KineoWorks device,
\li a set of obstacles: a list of Kcd objects,
\li a steering method: stored in the KwsDevice,
\li initial and goal configurations,
\li a roadmap : to store a roadmap
\li a roadmapBuilder : roadmap strategy
\li a pathOptimizer : to Optimise the path
*/
class ChppProblem {
public:
/**
* \brief Create a path planning problem with no initial nor goal configuration.
* \param inRobot robot associated to the path planning problem.
*/
ChppProblem(CkppDeviceComponentShPtr inRobot);
/**
* \brief Create a path planning problem with no initial nor goal configuration.
* \param inRobot robot associated to the path planning problem.
* \param inObstacleList list of obstacle of this problem.
*/
ChppProblem(CkppDeviceComponentShPtr inRobot,
const std::vector<CkcdObjectShPtr>& inObstacleList);
/**
\name Problem definition
@{
*/
/**
* \brief return shared pointer to robot.
*/
CkppDeviceComponentShPtr getRobot() const;
/**
* \brief Get shared pointer to initial configuration.
*/
CkwsConfigShPtr initConfig() const;
/**
* \brief Set initial configuration.
*/
void initConfig(CkwsConfigShPtr i_config);
/**
* \brief Get shared pointer to goal configuration.
*/
CkwsConfigShPtr goalConfig() const;
/**
* \brief Set goal configuration.
*/
void goalConfig(CkwsConfigShPtr i_config);
/**
\brief Set device steering method
*/
/**
*@}
*/
/**
\name Obstacles
@{
*/
/**
* \brief Store a copy of the list of obstacles.
* \param inCollisionList list of obstacles to be taken into account for this problem.
*
* Replaces previous list of obstacle if any.
* \note Compute collision entities.
*/
ktStatus obstacleList(const std::vector<CkcdObjectShPtr>& inCollisionList);
/**
* \brief return a shared pointer to the obstacle list
*/
const std::vector<CkcdObjectShPtr>& obstacleList();
/**
* \brief Add obstacle to the list.
* \param inObject a new object.
* \note Compute collision entities.
*/
ktStatus addObstacle(const CkcdObjectShPtr& inObject);
/**
*@}
*/
/**
\name Problem resolution
@{
*/
/**
\brief set device steering method
*/
void steeringMethod(const CkwsSteeringMethodShPtr &inSteeringMethod);
/**
\brief Get device steering method
*/
CkwsSteeringMethodShPtr steeringMethod() const;
/**
\brief Set roadmap building strategy.
*/
void roadmapBuilder( CkwsRoadmapBuilderShPtr inroadmapBuilder);
/**
\brief Get roadmap building strategy.
*/
CkwsRoadmapBuilderShPtr roadmapBuilder() ;
/**
\brief set roadmap.
\param inRoadmap shared pointer to a roadmap.
*/
void roadmap(CkwsRoadmapShPtr inRoadmap) ;
/**
\brief Get roadmap.
\return shared pointer to the roadmap.
*/
CkwsRoadmapShPtr roadmap() const;
/**
\brief Set pathOptimizer.
\param inPathOptimizer path optimizer.
*/
void pathOptimizer(CkwsPathOptimizerShPtr inPathOptimizer);
/**
\brief Get path optimizer
\return shared pointer to the path optimiser
*/
CkwsPathOptimizerShPtr pathOptimizer() ;
/**
\brief Add a path to the vector.
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_ADD_PATH.
*/
void addPath(CkwsPathShPtr kwsPath);
/**
\brief Get I-th path in vector
*/
CkwsPathShPtr getIthPath(unsigned int pathId) const;
/**
\brief et number of paths in vector
*/
unsigned int getNbPaths() const;
/**
*@}
*/
private :
/**
\brief pointer to a KineoWorks notificator.
*/
CkitNotificatorShPtr attNotificator;
/**
\brief the robot is a KineoWorks Device.
*/
CkppDeviceComponentShPtr attRobot;
/**
\brief Shared pointer to initial configuration.
*/
CkwsConfigShPtr attInitConf;
/**
\brief Shared pointer to goal configuration.
*/
CkwsConfigShPtr attGoalConf;
/**
\brief Shared pointer to a roadmap associate to the robot
*/
CkwsRoadmapShPtr attRoadmap ;
/**
\brief Shared pointer to a roadmapBuilder associate to the the robot and the roadMap
*/
CkwsRoadmapBuilderShPtr attRoadmapBuilder;
/**
\brief Shared pointer to a optimizer for the path
*/
CkwsPathOptimizerShPtr attPathOptimizer ;
/**
\brief The set of mObstacleList are likely to be a copy or reference to the list of obstacles of ChppPlanner.
However, to allow more general motion planning strategies, we
leave to possiblity to define it differently. Obstacles are a
list of KCD objects.
*/
std::vector< CkcdObjectShPtr > attObstacleList;
/**
\brief A vector of paths corresponding to this problem.
*/
std::vector<CkwsPathShPtr> attPathVector;
public:
// for notification:
static const CkitNotification::TType ID_HPP_ADD_PATH;
// key to retrieve
static const std::string PATH_KEY;
static const std::string PATH_ID_KEY;
static const std::string DEVICE_KEY;
class ChppProblem
{
public:
/**
* \brief Create a path planning problem with no initial nor goal configuration.
* \param inRobot robot associated to the path planning problem.
*/
ChppProblem ( CkppDeviceComponentShPtr inRobot );
/**
* \brief Create a path planning problem with no initial nor goal configuration.
* \param inRobot robot associated to the path planning problem.
* \param inObstacleList list of obstacle of this problem.
*/
ChppProblem ( CkppDeviceComponentShPtr inRobot,
const std::vector<CkcdObjectShPtr>& inObstacleList );
/**
\name Problem definition
@{
*/
/**
* \brief return shared pointer to robot.
*/
CkppDeviceComponentShPtr getRobot() const;
/**
* \brief Get shared pointer to initial configuration.
*/
CkwsConfigShPtr initConfig() const;
/**
* \brief Set initial configuration.
*/
void initConfig ( CkwsConfigShPtr i_config );
/**
* \brief Get shared pointer to goal configuration.
*/
CkwsConfigShPtr goalConfig() const;
/**
* \brief Set goal configuration.
*/
void goalConfig ( CkwsConfigShPtr i_config );
/**
\brief Set device steering method
*/
/**
*@}
*/
/**
\name Obstacles
@{
*/
/**
* \brief Store a copy of the list of obstacles.
* \param inCollisionList list of obstacles to be taken into account for this problem.
*
* Replaces previous list of obstacle if any.
* \note Compute collision entities.
*/
ktStatus obstacleList ( const std::vector<CkcdObjectShPtr>& inCollisionList );
/**
* \brief return a shared pointer to the obstacle list
*/
const std::vector<CkcdObjectShPtr>& obstacleList();
/**
* \brief Add obstacle to the list.
* \param inObject a new object.
* \note Compute collision entities.
*/
ktStatus addObstacle ( const CkcdObjectShPtr& inObject );
/**
*@}
*/
/**
\name Problem resolution
@{
*/
/**
\brief set device steering method
*/
void steeringMethod ( const CkwsSteeringMethodShPtr &inSteeringMethod );
/**
\brief Get device steering method
*/
CkwsSteeringMethodShPtr steeringMethod() const;
/**
\brief Set roadmap building strategy.
*/
void roadmapBuilder ( CkwsRoadmapBuilderShPtr inroadmapBuilder );
/**
\brief Get roadmap building strategy.
*/
CkwsRoadmapBuilderShPtr roadmapBuilder() ;
/**
\brief set roadmap.
\param inRoadmap shared pointer to a roadmap.
*/
void roadmap ( CkwsRoadmapShPtr inRoadmap ) ;
/**
\brief Get roadmap.
\return shared pointer to the roadmap.
*/
CkwsRoadmapShPtr roadmap() const;
/**
\brief Set pathOptimizer.
\param inPathOptimizer path optimizer.
*/
void pathOptimizer ( CkwsPathOptimizerShPtr inPathOptimizer );
/**
\brief Get path optimizer
\return shared pointer to the path optimiser
*/
CkwsPathOptimizerShPtr pathOptimizer() ;
/**
\brief Add a path to the vector.
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_ADD_PATH.
*/
void addPath ( CkwsPathShPtr kwsPath );
/**
\brief Get I-th path in vector
*/
CkwsPathShPtr getIthPath ( unsigned int pathId ) const;
/**
\brief et number of paths in vector
*/
unsigned int getNbPaths() const;
/**
\brief et initialize the map of innerBody (attMapOuter)
*/
void initMapOuter();
/**
*@}
*/
private :
/**
\brief pointer to a KineoWorks notificator.
*/
CkitNotificatorShPtr attNotificator;
/**
\brief the robot is a KineoWorks Device.
*/
CkppDeviceComponentShPtr attRobot;
/**
\brief Shared pointer to initial configuration.
*/
CkwsConfigShPtr attInitConf;
/**
\brief Shared pointer to goal configuration.
*/
CkwsConfigShPtr attGoalConf;
/**
\brief Shared pointer to a roadmap associate to the robot
*/
CkwsRoadmapShPtr attRoadmap ;
/**
\brief Shared pointer to a roadmapBuilder associate to the the robot and the roadMap
*/
CkwsRoadmapBuilderShPtr attRoadmapBuilder;
/**
\brief Shared pointer to a optimizer for the path
*/
CkwsPathOptimizerShPtr attPathOptimizer ;
/**
\brief The set of mObstacleList are likely to be a copy or reference to the list of obstacles of ChppPlanner.
However, to allow more general motion planning strategies, we
leave to possiblity to define it differently. Obstacles are a
list of KCD objects.
*/
std::vector< CkcdObjectShPtr > attObstacleList;
/**
\brief A vector of paths corresponding to this problem.
*/
std::vector<CkwsPathShPtr> attPathVector;
/**
\brief A map of each body to the a vector of its outer bodies.
*/
std::map<CkwsKCDBodyShPtr,std::vector<CkcdObjectShPtr> > attMapOuter;
public:
// for notification:
static const CkitNotification::TType ID_HPP_ADD_PATH;
// key to retrieve
static const std::string PATH_KEY;
static const std::string PATH_ID_KEY;
static const std::string DEVICE_KEY;
};
......
......@@ -15,11 +15,11 @@
#include "hppCore/hppProblem.h"
#include "hppModel/hppBody.h"
const CkitNotification::TType ChppProblem::ID_HPP_ADD_PATH(CkitNotification::makeID());
const CkitNotification::TType ChppProblem::ID_HPP_ADD_PATH ( CkitNotification::makeID() );
const std::string ChppProblem::PATH_KEY("path");
const std::string ChppProblem::PATH_ID_KEY("path_id");
const std::string ChppProblem::DEVICE_KEY("device");
const std::string ChppProblem::PATH_KEY ( "path" );
const std::string ChppProblem::PATH_ID_KEY ( "path_id" );
const std::string ChppProblem::DEVICE_KEY ( "device" );
/*****************************************
PUBLIC METHODS
......@@ -27,226 +27,303 @@ const std::string ChppProblem::DEVICE_KEY("device");
// ==========================================================================
ChppProblem::ChppProblem(CkppDeviceComponentShPtr inRobot) :
attNotificator(CkitNotificator::defaultNotificator()),
attRobot(inRobot)
ChppProblem::ChppProblem ( CkppDeviceComponentShPtr inRobot ) :
attNotificator ( CkitNotificator::defaultNotificator() ),
attRobot ( inRobot )
{
initMapOuter();
}
// ==========================================================================
ChppProblem::ChppProblem(CkppDeviceComponentShPtr inRobot,
const std::vector<CkcdObjectShPtr>& inObstacleList) :
attNotificator(CkitNotificator::defaultNotificator()),
attRobot(inRobot)
ChppProblem::ChppProblem ( CkppDeviceComponentShPtr inRobot,
const std::vector<CkcdObjectShPtr>& inObstacleList ) :
attNotificator ( CkitNotificator::defaultNotificator() ),
attRobot ( inRobot )
{
obstacleList(inObstacleList);
initMapOuter();
obstacleList ( inObstacleList );
}
// ==========================================================================
void ChppProblem::initMapOuter()
{
CkwsDevice::TBodyVector bodyVector;
attRobot->getBodyVector ( bodyVector );
// Loop over bodies of robot.
for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin(); bodyIter < bodyVector.end(); bodyIter++ )
{
// Try to cast body into CkwsKCDBody
CkwsKCDBodyShPtr kcdBody;
if ( kcdBody = boost::dynamic_pointer_cast<CkwsKCDBody> ( *bodyIter ) )
{
// Copy list of outer objects
attMapOuter[kcdBody]= kcdBody->outerObjects();
}
}
}
// ==========================================================================
CkppDeviceComponentShPtr ChppProblem::getRobot() const
{
return attRobot;
return attRobot;
}
// ==========================================================================
CkwsConfigShPtr ChppProblem::initConfig() const
{
return attInitConf;
return attInitConf;
}
// ==========================================================================
void ChppProblem::initConfig(CkwsConfigShPtr inConfig)
void ChppProblem::initConfig ( CkwsConfigShPtr inConfig )
{
attInitConf = inConfig;
attInitConf = inConfig;
}
// ==========================================================================
CkwsConfigShPtr ChppProblem::goalConfig() const
{
return attGoalConf;
return attGoalConf;
}
// ==========================================================================
void ChppProblem::goalConfig(CkwsConfigShPtr inConfig)
void ChppProblem::goalConfig ( CkwsConfigShPtr inConfig )
{
attGoalConf = inConfig;
attGoalConf = inConfig;
}
// ==========================================================================
ktStatus ChppProblem::obstacleList(const std::vector<CkcdObjectShPtr>& inCollisionList)
ktStatus ChppProblem::obstacleList ( const std::vector<CkcdObjectShPtr>& inCollisionList )
{
// Copy collision list in problem.
attObstacleList = inCollisionList;
// For each obstacle,
// - insert the obstacle in outer object of each body of the robot.
// Loop over object in the collision list.
// Get robot vector of bodies.
CkwsDevice::TBodyVector bodyVector;
attRobot->getBodyVector(bodyVector);
// Loop over bodies of robot.
for (CkwsDevice::TBodyIterator bodyIter = bodyVector.begin(); bodyIter < bodyVector.end(); bodyIter++) {
// Try to cast body into CkwsKCDBody
CkwsKCDBodyShPtr kcdBody;
if (kcdBody = boost::dynamic_pointer_cast<CkwsKCDBody>(*bodyIter)) {
// Copy list of outer objects
std::vector<CkcdObjectShPtr> collisionList = kcdBody->outerObjects();
unsigned int nObjects = attObstacleList.size();
for (unsigned int iObject=0; iObject<nObjects; iObject++) {
CkcdObjectShPtr kcdObject = attObstacleList[iObject];
// Add object to list of outer objects
collisionList.push_back(kcdObject);
}
// Set updated list as new list of outer objects
kcdBody->outerObjects(collisionList);
}
else {
std::cout << "CcppProblem::obstacleList: body is not KCD body. Obstacle not inserted." << std::endl;
}
}
return KD_OK;
// Copy collision list in problem.
attObstacleList = inCollisionList;
// For each obstacle,
// - insert the obstacle in outer object of each body of the robot.
// Loop over object in the collision list.
// Get robot vector of bodies.
CkwsDevice::TBodyVector bodyVector;
attRobot->getBodyVector ( bodyVector );
// Loop over bodies of robot.
for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin(); bodyIter < bodyVector.end(); bodyIter++ )
{
// Try to cast body into CkwsKCDBody
CkwsKCDBodyShPtr kcdBody;
if ( kcdBody = boost::dynamic_pointer_cast<CkwsKCDBody> ( *bodyIter ) )
{
// Copy list of outer objects
std::vector<CkcdObjectShPtr> collisionList = attMapOuter[kcdBody];
unsigned int nObjects = attObstacleList.size();
for ( unsigned int iObject=0; iObject<nObjects; iObject++ )
{
CkcdObjectShPtr kcdObject = attObstacleList[iObject];
// Add object to list of outer objects
collisionList.push_back ( kcdObject );
}
// Set updated list as new list of outer objects
kcdBody->outerObjects ( collisionList );
}
else
{
std::cout << "CcppProblem::obstacleList: body is not KCD body. Obstacle not inserted." << std::endl;
}
}
//----------------------------------
/*
// Copy collision list in problem.
attObstacleList = inCollisionList;
// For each obstacle,
// - insert the obstacle in outer object of each body of the robot.
// Loop over object in the collision list.
// Get robot vector of bodies.
CkwsDevice::TBodyVector bodyVector;
attRobot->getBodyVector ( bodyVector );
// Loop over bodies of robot.
for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin(); bodyIter < bodyVector.end(); bodyIter++ )
{
// Try to cast body into CkwsKCDBody
CkwsKCDBodyShPtr kcdBody;
if ( kcdBody = boost::dynamic_pointer_cast<CkwsKCDBody> ( *bodyIter ) )
{
// Copy list of outer objects
std::vector<CkcdObjectShPtr> collisionList = kcdBody->outerObjects();
unsigned int nObjects = attObstacleList.size();
for ( unsigned int iObject=0; iObject<nObjects; iObject++ )
{
CkcdObjectShPtr kcdObject = attObstacleList[iObject];
// Add object to list of outer objects
collisionList.push_back ( kcdObject );
}
// Set updated list as new list of outer objects
kcdBody->outerObjects ( collisionList );
}
else
{
std::cout << "CcppProblem::obstacleList: body is not KCD body. Obstacle not inserted." << std::endl;
}
}
*/
//----------------------------------
return KD_OK;