template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> bool
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const
{
const PointT tempPoint = getPointByIndex (data_arg);
// generate key for point
genOctreeKeyforPoint (tempPoint, key_arg);
return (true);
}
void TrajectoryController::addPoint(unsigned long now, /* current time of caller */
TimedPosition& pEye /* included time is used */,
unsigned long startApproachingTime /* = 0 */) {
if ((startApproachingTime == 0) || (startApproachingTime > pEye.atTime))
startApproachingTime = pEye.atTime;
TimedPosition eye(pEye);
if (debugTrajectory) {
eye.println("addPoint");
/*
bool saveDebugKinematic = debugKinematic;
debugKinematic = true;
Kinematics::getInstance().moveServosTo(pEye.pos);
debugKinematic = saveDebugKinematic;
*/
}
// check for boundaries first
Kinematics::getInstance().limitPosition(eye.pos);
// compute the point where we are at starting point.
// Returns null point if current trajectory ends before startApproachingTime
Position startPoint = getPointByTime(startApproachingTime);
// is new point within the first interval that is currently running?
bool pointIsInRunningCurve = (trajectory.size() >= 2) && (getPointByIndex(1).laterThan(startApproachingTime));
// is new point within the next interval that is currently running?
bool pointIsRightAfterRunningCurve = ((trajectory.size() >= 3) &&
getPointByIndex(1).earlierThan(startApproachingTime) &&
getPointByIndex(2).laterThan(startApproachingTime)) ||
((trajectory.size() == 2) && getPointByIndex(1).earlierThan(startApproachingTime));
// if movement start before last point, we have to change the current trajectory
if (startApproachingTime < getLastTrajectoryEntry().atTime) {
// remove all trajectory points happening after the starting time
trajectory.removeLaterThan(startApproachingTime);
// if the starting point happens earlier than the target position
// add the predicted position at the starting time
// (the new point is added anyhow later on)
// then add an intermediate point the movement starts from
if (startApproachingTime < eye.atTime) {
// add starting point of new movement
TimedPosition intermediatePoint(startApproachingTime,startPoint);
trajectory.add(intermediatePoint);
intermediatePoint.println("intermediate point");
}
}
// add new position
trajectory.add(eye);
// we need at least two positions to move
if (trajectory.size() >= 2) {
if (!bezierCurve.isActive())
initMovement(now,eye.atTime); // we start to move
else {
if (pointIsInRunningCurve || pointIsRightAfterRunningCurve) {
adaptCurrentTrajectoryPiece(now); // we adapt current curve
}
}
}
}
