AREXPORT bool ArServerInfoDrawings::addRobotsRangeDevices(ArRobot *robot)
{
std::list<ArRangeDevice *>::iterator it;
bool ret = true;
ArRangeDevice *device;
//printf("0\n");
if (robot == NULL || robot->getRangeDeviceList() == NULL)
{
ArLog::log(ArLog::Terse, "InfoDrawings::addRobotsRangeDevices: Robot or robot's range device list is NULL");
return false;
}
//printf("1\n");
for (it = robot->getRangeDeviceList()->begin();
it != robot->getRangeDeviceList()->end();
it++)
{
device = (*it);
//printf("2 %s\n", device->getName());
device->lockDevice();
if (!addRangeDevice(device))
ret = false;
device->unlockDevice();
}
//printf("3\n");
return ret;
}
AREXPORT void ArServerInfoSensor::getSensorCurrent(ArServerClient *client,
ArNetPacket *packet)
{
ArRangeDevice *dev;
char sensor[512];
std::list<ArPoseWithTime *> *readings;
std::list<ArPoseWithTime *>::iterator it;
while (packet->getDataLength() > packet->getDataReadLength())
{
ArNetPacket sendPacket;
// find out the sensor they want
packet->bufToStr(sensor, sizeof(sensor));
myRobot->lock();
if ((dev = myRobot->findRangeDevice(sensor)) == NULL)
{
myRobot->unlock();
ArLog::log(ArLog::Verbose, "ArServerInfoSensor::getSensorCurrent: No sensor %s", sensor);
sendPacket.byte2ToBuf(-1);
sendPacket.strToBuf(sensor);
client->sendPacketUdp(&sendPacket);
continue;
}
myRobot->unlock();
dev->lockDevice();
readings = dev->getCurrentBuffer();
if (readings == NULL)
{
dev->unlockDevice();
ArLog::log(ArLog::Verbose, "ArServerInfoSensor::getSensorCurrent: No current buffer for %s", sensor);
sendPacket.byte2ToBuf(0);
sendPacket.strToBuf(sensor);
client->sendPacketUdp(&sendPacket);
continue;
}
sendPacket.byte2ToBuf(readings->size());
sendPacket.strToBuf(sensor);
for (it = readings->begin(); it != readings->end(); it++)
{
sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getX()));
sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getY()));
}
dev->unlockDevice();
client->sendPacketUdp(&sendPacket);
}
}
/*
This is the guts of the action.
*/
ArActionDesired *ActionTurn::fire(ArActionDesired currentDesired)
{
double leftRange, rightRange;
// reset the actionDesired (must be done)
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// Get the left readings and right readings off of the sonar
leftRange = (mySonar->currentReadingPolar(0, 100) -
myRobot->getRobotRadius());
rightRange = (mySonar->currentReadingPolar(-100, 0) -
myRobot->getRobotRadius());
// if neither left nor right range is within the turn threshold,
// reset the turning variable and don't turn
if (leftRange > myTurnThreshold && rightRange > myTurnThreshold)
{
myTurning = 0;
myDesired.setRotVel(0);
}
// if we're already turning some direction, keep turning that direction
else if (myTurning)
{
myDesired.setRotVel(myTurnAmount * myTurning);
}
// if we're not turning already, but need to, and left is closer, turn right
// and set the turning variable so we turn the same direction for as long as
// we need to
else if (leftRange < rightRange)
{
myTurning = -1;
myDesired.setRotVel(myTurnAmount * myTurning);
}
// if we're not turning already, but need to, and right is closer, turn left
// and set the turning variable so we turn the same direction for as long as
// we need to
else
{
myTurning = 1;
myDesired.setRotVel(myTurnAmount * myTurning);
}
// return a pointer to the actionDesired, so resolver knows what to do
return &myDesired;
}
virtual ArActionDesired *fire (ArActionDesired currentDesired)
{
static bool BangBang = false;
double LeftFrontSonar = mysonar->currentReadingPolar(0,90);
double LeftBackSonar = mysonar->currentReadingPolar(90,180);
double RightBackSonar = mysonar->currentReadingPolar(180,270);
double RightFrontSonar = mysonar->currentReadingPolar(270,360);//- myRobot->getRobotRadius();// - robot.getRobotRadius();
double leftwheel =0,rightwheel =0;
double turnleft=0,turnright=0;
double forward=0,backward=0;
Emergency=false;
if(LeftFrontSonar<SonarThreshold) {turnleft-=2;backward-=100;Emergency=true;}
if(RightFrontSonar<SonarThreshold) {turnright+=2;backward-=100;Emergency=true;}
if(LeftBackSonar<SonarThreshold) {turnright+=2;forward+=100;Emergency=true;}
if(RightBackSonar<SonarThreshold) {turnleft-=2;forward+=100;Emergency=true;}
Emergency=false;
if (kbhit())
{
char mychar = getch();
if(mychar=='q'){myRobot->disconnect();Aria::shutdown();}
if(mychar=='w'){myDesired.setVel(+200);}
if(mychar=='s'){myDesired.setVel(-200);}
if(mychar=='a'){myDesired.setDeltaHeading(+10,1);}
if(mychar=='d'){myDesired.setDeltaHeading(-10,1);}
BangBang=true;
}
else if(Emergency==true)
{
// myDesired.setDeltaHeading(turnleft+turnright,1);
// myDesired.setVel(forward+backward);
// myDesired.setVel(0);
}
else
{
if(BangBang==true)
{
puts("resetting to zero");
myDesired.setDeltaHeading(0,1);
myDesired.setVel(0);
myDesired.reset();
}
BangBang=false;
}
return &myDesired;
}
/*
This is the guts of the Turn action.
*/
ArActionDesired *ActionTurns::fire(ArActionDesired currentDesired)
{
double leftRange, rightRange;
// reset the actionDesired (must be done)
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// Get the left readings and right readings off of the sonar
leftRange = (mySonar->currentReadingPolar(0, 100) -
myRobot->getRobotRadius());
rightRange = (mySonar->currentReadingPolar(-100, 0) -
myRobot->getRobotRadius());
// si es que el activador esta en cero que resetee el turn y que no se mueva
if (myActivate == 0)
{
myTurning = 0;
myDesired.setDeltaHeading(0);
}
// if we're already turning some direction, keep turning that direction
else if (myTurning)
{
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// Gira a la izquierda
else if (myDirection == 2)
{
myTurning = -1;
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// Gira a la derecha
else if (myDirection == 1)
{
myTurning = 1;
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// return a pointer to the actionDesired, so resolver knows what to do
return &myDesired;
}
/*
This fire is the whole point of the action.
currentDesired is the combined desired action from other actions
previously processed by the action resolver. In this case, we're
not interested in that, we will set our desired
forward velocity in the myDesired member, and return it.
Note that myDesired must be a class member, since this method
will return a pointer to myDesired to the caller. If we had
declared the desired action as a local variable in this method,
the pointer we returned would be invalid after this method
returned.
*/
ArActionDesired *ActionGos::fire(ArActionDesired currentDesired)
{
double range;
double speed;
// reset the actionDesired (must be done), to clear
// its previous values.
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// get the range of the sonar
range = mySonar->currentReadingPolar(-70, 70) - myRobot->getRobotRadius();
// if the range is greater than the stop distance, find some speed to go
if (range > myStopDistance)
{
// just an arbitrary speed based on the range
speed = range * .3;
// if that speed is greater than our max, cap it
if (speed > myMaxSpeed)
speed = myMaxSpeed;
// now set the velocity
myDesired.setVel(speed);
}
// the range was less than the stop distance, so request stop
else
{
myDesired.setVel(0);
myFound = 1;
}
// return a pointer to the actionDesired to the resolver to make our request
return &myDesired;
}
void Joydrive::drive(void)
{
int trans, rot;
ArPose pose;
ArPose rpose;
ArTransform transform;
ArRangeDevice *dev;
ArSensorReading *son;
if (!myRobot->isConnected())
{
printf("Lost connection to the robot, exiting\n");
exit(0);
}
printf("\rx %6.1f y %6.1f th %6.1f",
myRobot->getX(), myRobot->getY(), myRobot->getTh());
fflush(stdout);
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
{
if (ArMath::fabs(myRobot->getVel()) < 10.0)
myRobot->comInt(ArCommands::ENABLE, 1);
myJoyHandler.getAdjusted(&rot, &trans);
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
time(NULL) - myLastPress > 1)
{
myLastPress = time(NULL);
printf("\n");
switch (myTest)
{
case 1:
printf("Moving back to the origin.\n");
pose.setPose(0, 0, 0);
myRobot->moveTo(pose);
break;
case 2:
printf("Moving over a meter.\n");
pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
myRobot->moveTo(pose);
break;
case 3:
printf("Doing a transform test....\n");
printf("\nOrigin should be transformed to the robots coords.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(0, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
printf("\nRobot coords should be transformed to the origin.\n");
transform = myRobot->getToLocalTransform();
pose = myRobot->getPose();
pose = transform.doTransform(pose);
rpose.setPose(0, 0, 0);
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
break;
case 4:
printf("Doing a tranform test...\n");
printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(-1000, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
printf("Probable Success\n");
else
printf("#### FAILURE\n");
break;
case 5:
printf("Doing a transform test on range devices..\n");
printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
dev = myRobot->findRangeDevice("sonar");
if (dev == NULL)
{
printf("No sonar on the robot, can't do the test.\n");
break;
}
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
pose = myRobot->getPose();
pose.setX(pose.getX() + 4000);
pose.setY(pose.getY() + 4000);
myRobot->moveTo(pose);
printf("Moved robot.\n");
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
break;
case 6:
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
printf("Disconnecting from the robot, then reconnecting.\n");
myRobot->disconnect();
myRobot->blockingConnect();
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
break;
default:
printf("No test for second button.\n");
break;
}
}
}