def getRoadmapNodeFromConfig(self, config, numConnectNeighbors=5, allowMoveIt=True):
""" Searches for the closest node in the roadmap. If this node is
very close to the given configuration, the node is returned.
Otherwise it attempts to connect the given configuration to one of the
numConnectNeighbors closest neighbors in the roadmap
@param config - configuration for which we want a roadmap node
@param numConnectNeighbors - number of nearest neighbors to attempt a connection to,
if config is not very close to any roadmap node.
@return a roadmap node from which we can plan a roadmap path or None if nothing worked.
"""
self._preemptionCheck()
nearestNeighbors = self._roadmap.getNearestNeighbors(config, nNeighbors=numConnectNeighbors)
if len(nearestNeighbors) == 0:
raise ValueError('No nearest neighbor found for configuration ' + str(config) +
'. The roadmap seems to be empty!')
# let's first check whether we are at a configuration that is already in the roadmap
for nn in nearestNeighbors:
rospy.loginfo('Nearest neighbor: ' + nn.getName())
nn = nearestNeighbors[0]
if roadmap.computeDistanceConfigs(config, nn.getConfig()) < CONFIGURATION_EPSILON:
return nn
# if we reach this point, then we need to connect config to any of our nearest neighbors
# first try doing this by linear interpolation
startNode = roadmap.RoadmapNode(config=config, name=self._roadmap.getNewNodeName())
(connecting_traj, idx) = self._planPathToAny(start=startNode.getConfig(),
goals=map(lambda x: x.getConfig(), nearestNeighbors),
allowMoveIt=allowMoveIt)
if connecting_traj is not None:
if idx not in range(len(nearestNeighbors)):
raise ValueError('Received a non None trajectory, but not a valid index for the goal config.')
endNode = nearestNeighbors[idx]
startNode.addEdge(endNode, connecting_traj)
self._roadmap.addNode(startNode)
self._addNodeToPoseCache(startNode)
invTraj = self._invertTrajectory(connecting_traj)
endNode.addEdge(startNode, invTraj)
return startNode
return None
def configurationsAreIdentical(configA, configB):
return roadmap.computeDistanceConfigs(configA, configB) < CONFIGURATION_EPSILON
