/** returns a new capabilities object (Capabilities) */
static jobject
CreateNewCapObject (JNIEnv *env) {
jobject cap,obj;
if ((*env)->PushLocalFrame(env, 20))
return NULL;
LOCAL_DEBUG(JNI_TRUE, DEBUG_VERBOSE, ("creating new cap object"));
cap = (*env)->NewObject(env, iscClass, iscConstId);
/* now fill in the billing capabilities */
LOCAL_DEBUG(JNI_TRUE, DEBUG_VERBOSE, ("creating billing capabilities"));
obj = (*env)->GetObjectField(env, cap, billingFid);
if (obj == NULL) {
LOCAL_DEBUG(JNI_TRUE, DEBUG_ERROR, ("error getting billing capabilities"));
(*env)->PopLocalFrame(env, NULL);
return NULL;
}
if (SetBillingFields(env, obj)) {
(*env)->PopLocalFrame(env, NULL);
return NULL;
}
(*env)->DeleteLocalRef(env, obj);
return (*env)->PopLocalFrame(env, cap);
}
static int
SetBillingFields (JNIEnv *env, jobject billCap) {
jboolean result = JNI_FALSE;
if (radiusEnabled() > 0)
result = JNI_TRUE;
LOCAL_DEBUG(JNI_TRUE, DEBUG_VERBOSE, ("radius Enabled returns %s", (result == JNI_TRUE
)?"TRUE":"FALSE"));
/* fast start */
//(*env)->SetBooleanField(env, billCap, billRadiusFid, result?JNI_TRUE:JNI_FALSE);
(*env)->CallVoidMethod(env, billCap, setRadiusMid, result?JNI_TRUE:JNI_FALSE);
return 0;
}
bool BiRrt::plan(bool bQuiet)
{
if (!bQuiet)
{
SABA_INFO << "Starting BiRrt planner" << std::endl;
switch (rrtMode)
{
case eExtend:
cout << "-- ModeA: RRT-EXTEND" << endl;
break;
case eConnect:
cout << "-- ModeA: RRT-CONNECT" << endl;
break;
case eConnectCompletePath:
cout << "-- ModeA: RRT-CONNECT (only complete paths)" << endl;
break;
default:
break;
}
switch (rrtMode2)
{
case eExtend:
cout << "-- ModeB: RRT-EXTEND" << endl;
break;
case eConnect:
cout << "-- ModeB: RRT-CONNECT" << endl;
break;
case eConnectCompletePath:
cout << "-- ModeB: RRT-CONNECT (only complete paths)" << endl;
break;
default:
break;
}
}
if (!isInitialized())
{
SABA_ERROR << " planner: not initialized..." << std::endl;
return false;
}
cycles = 0;
int distChecksStart = cspace->performaceVars_distanceCheck;
int colChecksStart = cspace->performaceVars_collisionCheck;
bool found = false;
stopSearch = false;
clock_t startClock = clock();
solution.reset();
RobotPtr robot = cspace->getRobot();
bool bVisStatus = true;
if (robot)
{
bVisStatus = robot->getUpdateVisualizationStatus();
robot->setUpdateVisualization(false);
}
ExtensionResult extResultA;
ExtensionResult extResultB;
bool switched = false;
int *lastIDA = &lastAddedID;
int *lastIDB = &lastAddedID2;
CSpaceTreePtr treeA = tree;
CSpaceTreePtr treeB = tree2;
RrtMethod rrtModeA = rrtMode;
RrtMethod rrtModeB = rrtMode2;
// PLANNING LOOP
do
{
// CHOOSE A RANDOM CONFIGURATION (NOT GOAL DIRECTED, ONLY RANDOMLY)
cspace->getRandomConfig(tmpConfig, true);
LOCAL_DEBUG("----------------------------------------------------" << endl);
LOCAL_DEBUG("Random Conf:" << endl << tmpConfig << endl);
if (switched)
{
LOCAL_DEBUG ("SWITCHED" << endl);
lastIDA = &lastAddedID2;
lastIDB = &lastAddedID;
treeA = tree2;
treeB = tree;
rrtModeA = rrtMode2;
rrtModeB = rrtMode;
} else
{
lastIDA = &lastAddedID;
lastIDB = &lastAddedID2;
treeA = tree;
treeB = tree2;
rrtModeA = rrtMode;
rrtModeB = rrtMode2;
}
switch (rrtModeA)
{
case eExtend:
extResultA = extend(tmpConfig, treeA, *lastIDA);
break;
case eConnect:
extResultA = connectUntilCollision(tmpConfig, treeA, *lastIDA);
break;
case eConnectCompletePath:
extResultA = connectComplete(tmpConfig, treeA, *lastIDA);
break;
default:
break;
}
LOCAL_DEBUG ("ExtResultA:" << extResultA << endl);
if (extResultA==eError)
stopSearch = true;
if (extResultA==ePartial || extResultA==eSuccess)
{
// update config
LOCAL_DEBUG ("Last ID A:" << *lastIDA << endl);
CSpaceNodePtr n = treeA->getNode(*lastIDA);
tmpConfig = n->configuration;
LOCAL_DEBUG ("Tmp goal B:" << endl << tmpConfig << endl);
switch (rrtModeB)
{
case eExtend:
extResultB = extend(tmpConfig, treeB, *lastIDB);
break;
case eConnect:
extResultB = connectUntilCollision(tmpConfig, treeB, *lastIDB);
break;
case eConnectCompletePath:
extResultB = connectComplete(tmpConfig, treeB, *lastIDB);
break;
default:
break;
}
LOCAL_DEBUG ("Last ID B:" << *lastIDB << endl);
LOCAL_DEBUG ("ExtResultB:" << extResultB << endl);
if (extResultB==eError)
stopSearch = true;
if (extResultB==eSuccess)
{
goalNode = treeB->getNode(*lastIDB);
if (!goalNode)
{
SABA_ERROR << " no node for ID: " << lastIDB << endl;
stopSearch = true;
} else
{
found = true;
}
}
}
cycles++;
switched = !switched;
} while (!stopSearch && cycles<maxCycles && !found);
clock_t endClock = clock();
long diffClock = (long)(((float)(endClock - startClock) / (float)CLOCKS_PER_SEC) * 1000.0);
planningTime = (float)diffClock;
if (!bQuiet)
{
SABA_INFO << "Needed " << diffClock << " ms of processor time." << std::endl;
SABA_INFO << "Created " << tree->getNrOfNodes() << " + " << tree2->getNrOfNodes() << " = " << tree->getNrOfNodes()+tree2->getNrOfNodes() << " nodes." << std::endl;
SABA_INFO << "Collision Checks: " << (cspace->performaceVars_collisionCheck-colChecksStart) << std::endl;
SABA_INFO << "Distance Calculations: " << (cspace->performaceVars_distanceCheck-distChecksStart) << std::endl;
int nColChecks = (cspace->performaceVars_collisionCheck-colChecksStart);
if (diffClock>0)
{
float fPerf = (float)nColChecks / (float)diffClock * 1000.0f;
std::cout << "Performance: " << fPerf << " cps (collision-checks per second)." << std::endl;
}
}
if (robot && bVisStatus)
{
robot->setUpdateVisualization(bVisStatus);
}
if (found)
{
if (!bQuiet)
SABA_INFO << "Found RRT solution with " << cycles << " cycles."<< std::endl;
createSolution(bQuiet);
return true;
}
// something went wrong...
if (cycles>=maxCycles)
{
SABA_WARNING << " maxCycles exceeded..." << std::endl;
}
if (stopSearch)
{
SABA_WARNING << " search was stopped..." << std::endl;
}
return false;
}
