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 } } } }