void Control::plan(player_pose2d_t pos) {
double dx, dy;
dx = currentgoal->px - pos.px;
dy = currentgoal->py - pos.py;
distance = sqrt(dx*dx + dy*dy);
direction = atan2(dy, dx);
if( distance < 0.05 ) {
// We've arrived at the destination,
nextGoal();
dx = currentgoal->px - pos.px;
dy = currentgoal->py - pos.py;
distance = sqrt(dx*dx + dy*dy);
direction = atan2(dy, dx);
}
}
/* This is called at each goal point. It moves the robot forward a bit, waits 3 seconds
* then backs the mobile robot up. Then it tells the ARNL path planner to
* navigate to the next goal point in the chain (named Goal 0, Goal 1, Goal 2, etc.)
* The last goal point is special, no action and the chain of ARNL goals
* stops. The robot will wait at this goal point until sent to the first
* goal manually from MobileEyes.
*/
void runTask()
{
// Make copies of variables shared between threads
lock();
int currentGoal = myCurrentGoal;
int numGoals = myNumGoals;
int moveDist = myApproachDist;
unlock();
// if at end of chain, stop chain
if(currentGoal == numGoals)
{
ArLog::log(ArLog::Normal, "Waiting to be loaded, end of chain.");
if(myServerMode) myServerMode->setStatus("End of goal chain.");
lock();
myCurrentGoal = 1;
unlock();
return; // end of thread
}
/* In this example, we will move the robot forward a bit, wait, then move it
back.
In your application, you could do somethng here like trigger devices, cameras,
sensors, output, speech, etc. You can access the current goal name and
use that to control the activity, or use that goal name to obtain the
goal object from the map ArMap object which has additional properties of
the goal.
*/
// move forward a bit
ArLog::log(ArLog::Normal, "Moving forward a bit");
if(myServerMode) myServerMode->setStatus("Moving forward");
getRobot()->clearDirectMotion();
getRobot()->move(moveDist);
waitForMoveDone();
getRobot()->clearDirectMotion();
ArUtil::sleep(500);
// Wait a bit.
ArLog::log(ArLog::Normal, "Would do goal-specific task at goal %d.", currentGoal);
ArLog::log(ArLog::Normal, "Waiting 3 sec.");
if(myServerMode) myServerMode->setStatus("Waiting 3 sec");
ArUtil::sleep(3000);
// back up a bit
ArLog::log(ArLog::Normal, "Backing up a bit");
if(myServerMode) myServerMode->setStatus("Backing up a bit");
getRobot()->clearDirectMotion();
getRobot()->move(-moveDist);
waitForMoveDone();
getRobot()->clearDirectMotion();
ArUtil::sleep(500);
/* Go to the next goal in the chain. The name is assumed to be "Goal X"
where X is the goal index number. You could use another scheme for
naming goals, or you could store a list of strings in this class
*/
++currentGoal;
if(currentGoal > numGoals) currentGoal = 0;
char name[128];
snprintf(name, 127, "Goal %d", currentGoal);
ArLog::log(ArLog::Normal, "Going to next goal %s", name);
if(myServerMode) myServerMode->setStatus("ASyncTask example done. Going to next goal.");
nextGoal(name);
// Save the new goal index
lock();
myCurrentGoal = currentGoal;
unlock();
// This is the end of the thread.
return;
}
const ompl::base::State *ompl::base::PlannerInputStates::nextGoal()
{
// This initialization is safe since we are in a non-const function anyway.
static PlannerTerminationCondition ptc = plannerAlwaysTerminatingCondition();
return nextGoal(ptc);
}
const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(void)
{
static PlannerAlwaysTerminatingCondition ptc;
return nextGoal(ptc);
}
