void sendPoseRobot(ArServerClient* client, ArNetPacket* packet) {
ArNetPacket reply;
ArPose pose = gotoGoal.getPose();
reply.doubleToBuf(pose.getX());
reply.doubleToBuf(pose.getY());
client->sendPacketUdp(&reply);
}
/*-------------------------------------------------------------
getOdometryFull
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometryFull(
poses::CPose2D &out_odom,
double &out_lin_vel,
double &out_ang_vel,
int64_t &out_left_encoder_ticks,
int64_t &out_right_encoder_ticks
)
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
// Odometry:
ArPose pose = THE_ROBOT->getEncoderPose();
out_odom.x( pose.getX() * 0.001 );
out_odom.y( pose.getY() * 0.001 );
out_odom.phi( DEG2RAD( pose.getTh() ) );
// Velocities:
out_lin_vel = THE_ROBOT->getVel() * 0.001;
out_ang_vel = DEG2RAD( THE_ROBOT->getRotVel() );
// Encoders:
out_left_encoder_ticks = THE_ROBOT->getLeftEncoder();
out_right_encoder_ticks = THE_ROBOT->getRightEncoder();
THE_ROBOT->unlock();
#else
MRPT_UNUSED_PARAM(out_odom); MRPT_UNUSED_PARAM(out_lin_vel); MRPT_UNUSED_PARAM(out_ang_vel);
MRPT_UNUSED_PARAM(out_left_encoder_ticks); MRPT_UNUSED_PARAM(out_right_encoder_ticks);
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
/*!
* Callback function for the l key. Localizes the robot at the home position.
* Warning: Robot must be physically at the first home pose.
*/
void
lkeyCB(void)
{
roundRobinFlag = false;
static int once = 0;
ArPose pose;
if(!once && advancedptr->myHomeList.size()>0){
once = 1;
ArPose top = advancedptr->myHomeList.front()->getPose();
printf("Localizing at %s %5.2f %5.2f %5.2f\n",
advancedptr->myHomeList.front()->getName(),
top.getX(), top.getY(), top.getTh());
//
// Set the pose to localize the robot about.
//
pose = top;
} else {
advancedptr->myRobot->lock();
pose = advancedptr->myRobot->getPose();
advancedptr->myRobot->unlock();
}
//
// Do the localization at the current pose.
//
if(advancedptr->localizeAtSetPose(pose)){
printf("Localized at current pose: Score: %5.2f out of 1.0\n",
advancedptr->getLocalizationScore());
}else {
printf("Failed to localize\n");
}
}
/*!
* Callback function for the r key. Gets the robot to pick the next
* goal point and plan to it. and keep going around the list.
*/
void
rkeyCB(void)
{
if(roundRobinFlag == false){
roundRobinFlag = true;
}else{
roundRobinFlag = false;
return;
}
if(advancedptr->myLocaTask->getInitializedFlag()){
if(advancedptr->myGoalList.size()>0){
ArMapObject* front = advancedptr->myGoalList.front();
ArPose top = front->getPose();
bool headingFlag = false;
if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
headingFlag = true;
else
headingFlag = false;
printf("Path planing to goal %s %5.2f %5.2f %5.2f: %d poses\n",
front->getName(),
top.getX(), top.getY(), top.getTh(),
advancedptr->myGoalList.size());
advancedptr->myGoalList.pop_front();
advancedptr->myGoalList.push_back(front);
//
// Setup the pathplanning task to this destination.
//
advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
}
}else{
printf("Localize the robot first\n");
}
}
/**
* @brief read the robot's position and publish them to topic DEFAULT_TOPIC_ODOM
*/
void AriaCore::publish_pose()
{
ArPose pos;
m_robot.lock();
{
pos = m_robot.getPose();
}
m_robot.unlock();
// set position
nav_msgs::Odometry msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = m_frameIDOdom;
msg.child_frame_id = m_frameIDRobot;
tf::poseTFToMsg( tf::Pose(tf::Quaternion(0, 0, pos.getTh()*D2R),
tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), msg.pose.pose);
// set vel
m_robot.lock();
{
msg.twist.twist.linear.x = m_robot.getVel()/1000.0;
msg.twist.twist.angular.z= m_robot.getRotVel()*D2R;
}
m_robot.unlock();
m_pubPos.publish(msg);
}
/*-------------------------------------------------------------
getOdometryIncrement
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometryIncrement(
poses::CPose2D &out_incr_odom,
double &out_lin_vel,
double &out_ang_vel,
int64_t &out_incr_left_encoder_ticks,
int64_t &out_incr_right_encoder_ticks
)
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
static CPose2D last_pose;
static int64_t last_left_ticks=0, last_right_ticks=0;
CPose2D cur_pose;
int64_t cur_left_ticks, cur_right_ticks;
// Velocities:
out_lin_vel = THE_ROBOT->getVel() * 0.001;
out_ang_vel = DEG2RAD( THE_ROBOT->getRotVel() );
// Current odometry:
ArPose pose = THE_ROBOT->getEncoderPose();
cur_pose.x( pose.getX() * 0.001 );
cur_pose.y( pose.getY() * 0.001 );
cur_pose.phi( DEG2RAD( pose.getTh() ) );
// Current encoders:
cur_left_ticks = THE_ROBOT->getLeftEncoder();
cur_right_ticks = THE_ROBOT->getRightEncoder();
// Compute increment:
if (m_firstIncreOdometry)
{
// First time:
m_firstIncreOdometry = false;
out_incr_odom = CPose2D(0,0,0);
out_incr_left_encoder_ticks = 0;
out_incr_right_encoder_ticks = 0;
}
else
{
// Normal case:
out_incr_odom = cur_pose - last_pose;
out_incr_left_encoder_ticks = cur_left_ticks - last_left_ticks;
out_incr_right_encoder_ticks = cur_right_ticks - last_right_ticks;
}
// save for the next time:
last_pose = cur_pose;
last_left_ticks = cur_left_ticks;
last_right_ticks = cur_right_ticks;
THE_ROBOT->unlock();
#else
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
/**
@param pose the coord system from which we transform to abs world coords
*/
AREXPORT void ArTransform::setTransform(ArPose pose)
{
myTh = pose.getTh();
myCos = ArMath::cos(-myTh);
mySin = ArMath::sin(-myTh);
myX = pose.getX();
myY = pose.getY();
}
void RosArnlNode::simple_goal_sub_cb(const geometry_msgs::PoseStampedConstPtr &msg)
{
ArPose p = rosPoseToArPose(msg->pose);
bool heading = !ArMath::isNan(p.getTh());
ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Received goal %.0fmm, %.0fmm, %.0fdeg", p.getX(), p.getY(), p.getTh());
//arnl.pathTask->pathPlanToPose(p, true);
arnl.modeGoto->gotoPose(p, heading);
}
int main (int argc, char** argv) {
Aria::init();
Arnl::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
//ArAnalogGyro gyro = new
ArAnalogGyro gyro(&robot);
if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given, just exit.
Aria::logOptions();
Aria::exit(1);
return 1;
}
}
ArSonarDevice sonarDev;
robot.addRangeDevice(&sonarDev);
robot.runAsync(true);
ArMap map("office.map");
// set it up to ignore empty file names (otherwise if a configuration omits
// the map file, the whole configuration change will fail)
map.setIgnoreEmptyFileName(true);
// ignore the case, so that if someone is using MobileEyes or
// MobilePlanner from Windows and changes the case on a map name,
// it will still work.
map.setIgnoreCase(true);
ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
locTask.localizeRobotAtHomeBlocking();
robot.runAsync(true);
robot.enableMotors();
//robot.lock();
robot.setVel(200);
//robot.unlock();
ArPose pose;
locTask.forceUpdatePose(pose);
while(true) {
//while (robot.blockingConnect()){
//robot.lock();
//ArPose pose = robot.getPose();
//pose.setX(100);
//robot.moveTo(pose);
//t = robot.getLastOdometryTime();
//int a = interp.getPose(t, &pose);
ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
//robot.unlock();
}
}
geometry_msgs::Pose arPoseToRosPose(const ArPose& arpose)
{
// TODO use tf::poseTFToMsg instead?
geometry_msgs::Pose rospose;
rospose.position.x = arpose.getX() / 1000.0;
rospose.position.y = arpose.getY() / 1000.0;
rospose.position.z = 0;
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(arpose.getTh()*M_PI/180.0), rospose.orientation);
return rospose;
}
void testNotPerp(ArLineSegment *segment, ArPose perp, char *name)
{
ArPose pose;
if (segment->getPerpPoint(perp, &pose))
{
printf("%s was perp but shouldn't have been, at %.0f %.0f\n", name,
pose.getX(), pose.getY());
exit(1);
}
}
AREXPORT ArMapObject::ArMapObject(const char *type,
ArPose pose,
const char *description,
const char *iconName,
const char *name,
bool hasFromTo,
ArPose fromPose,
ArPose toPose) :
myType((type != NULL) ? type : ""),
myBaseType(),
myName((name != NULL) ? name : "" ),
myDescription((description != NULL) ? description : "" ),
myPose(pose),
myIconName((iconName != NULL) ? iconName : "" ),
myHasFromTo(hasFromTo),
myFromPose(fromPose),
myToPose(toPose),
myFromToSegments(),
myStringRepresentation()
{
if (myHasFromTo)
{
double angle = myPose.getTh();
double sa = ArMath::sin(angle);
double ca = ArMath::cos(angle);
double fx = fromPose.getX();
double fy = fromPose.getY();
double tx = toPose.getX();
double ty = toPose.getY();
ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
myFromToSegments.push_back(ArLineSegment(P0, P1));
myFromToSegments.push_back(ArLineSegment(P1, P2));
myFromToSegments.push_back(ArLineSegment(P2, P3));
myFromToSegments.push_back(ArLineSegment(P3, P0));
myFromToSegment.newEndPoints(fromPose, toPose);
}
else { // pose
size_t whPos = myType.rfind("WithHeading");
size_t whLen = 11;
if (whPos > 0) {
if (whPos == myType.size() - whLen) {
myBaseType = myType.substr(0, whPos);
}
}
} // end else pose
IFDEBUG(
ArLog::log(ArLog::Normal,
"ArMapObject::ctor() created %s (%s)",
myName.c_str(), myType.c_str());
);
AREXPORT void ArActionGotoStraight::setGoalRel(double dist,
double deltaHeading,
bool backToGoal,
bool justDistance)
{
ArPose goal;
goal.setX(dist * ArMath::cos(deltaHeading));
goal.setY(dist * ArMath::sin(deltaHeading));
goal = myRobot->getToGlobalTransform().doTransform(goal);
setGoal(goal, backToGoal, justDistance);
}
void testPerp(ArLineSegment *segment, ArPose perp, ArPose perpPoint,
char *name)
{
ArPose pose;
if (!segment->getPerpPoint(perp, &pose) ||
fabs(pose.getX() - perpPoint.getX()) > .001 ||
fabs(pose.getY() - perpPoint.getY()) > .001)
{
printf("%s wasn't perp but should have been\n", name);
exit(1);
}
}
/*!
* Interact with user on the terminal.
*/
void
interact()
{
ArMap* ariamap = advancedptr->myMap;
sleep(1);
advancedptr->getAllGoals(ariamap);
advancedptr->getAllRobotHomes(ariamap);
/// MPL
// lkeyCB();
advancedptr->myLocaTask->localizeRobotAtHomeNonBlocking();
//
// Interact with user using keyboard.
//
ArGlobalFunctor lCB(&lkeyCB);
ArGlobalFunctor pCB(&pkeyCB);
ArGlobalFunctor hCB(&hkeyCB);
ArGlobalFunctor rCB(&rkeyCB);
ArGlobalFunctor qCB(&quitCB);
ArGlobalFunctor escapeCB(&quitCB);
keyHandler.addKeyHandler('l', &lCB);
keyHandler.addKeyHandler('p', &pCB);
keyHandler.addKeyHandler('h', &hCB);
keyHandler.addKeyHandler('r', &rCB);
keyHandler.addKeyHandler('q', &qCB);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
printf("Put robot at RobotHome and press 'l' to localize first.\n\
Press 'p' to move to the next goal\n\
Press 'h' to move to the first home\n\
Press 'r' to move to the goals in order\n\
Press 'q' to quit\n");
while (advancedptr->myLocaTask->getRunning() &&
advancedptr->myPathPlanningTask->getRunning()){
keyHandler.checkKeys();
ArUtil::sleep(250);
advancedptr->myRobot->lock();
ArPose rpose = advancedptr->myRobot->getPose();
double lvel = advancedptr->myRobot->getVel();
double avel = advancedptr->myRobot->getRotVel();
double volts = advancedptr->myRobot->getBatteryVoltage();
advancedptr->myRobot->unlock();
if(advancedptr->myLocaTask->getInitializedFlag()){
printf("\r%5.2f %5.2f %5.2f: %5.2f %5.2f: %4.1f\r",
rpose.getX(), rpose.getY(), rpose.getTh(), lvel, avel, volts);
fflush(stdout);
}
}
}
// Method called by accessor methods when properties changed. This reconstructs
// the myReply packet sent in response to requests from clients
void Circle::regenerate()
{
myMutex.lock();
myReply.empty();
myReply.byte4ToBuf(myNumPoints);
double a = 360.0/myNumPoints;
for(unsigned int i = 0; i < myNumPoints; ++i)
{
myReply.byte4ToBuf(ArMath::roundInt(myPos.getX()+ArMath::cos(i*a)*myRadius)); // X
myReply.byte4ToBuf(ArMath::roundInt(myPos.getY()+ArMath::sin(i*a)*myRadius)); // Y
}
myMutex.unlock();
}
int ballDetection::moveToBall() {
cout << "rufe Movetoball auf"<<endl;
//cout << "KoordX: "<< KoordX<<endl;
//int headingCount = 0;
double obj = findObj();
// kleiner wählen damit auf ball in größerer Entfernung gefunden wird
while(obj >= 0.5) {
p3dxptr->comInt(ArCommands::VEL, 0);
//ballimBild = true;
//cout << "#Drehungen: "<< headingCount << endl;
ArPose currentPose = p3dxptr->getPose();
cout<<"Koorigiere Heading "<<currentPose.getTh()<<endl;
if(KoordX >= 0 && KoordX <= 399) {
//p3dx.comInt(ArCommands::HEAD, i);
cout<<"Setze Heading -10 "<<endl;
// headingCount++;
p3dxptr->comInt(ArCommands::HEAD, (currentPose.getTh()+5));
ArUtil::sleep(500);
}
if(KoordX > 399 && KoordX <= 799) {
//p3dx.comInt(ArCommands::HEAD, i);
cout<<"Setze Heading +10 "<<endl;
// headingCount++;
p3dxptr->comInt(ArCommands::HEAD, currentPose.getTh()-5);
ArUtil::sleep(500);
}
if(rotateToBall() == 1) {
cout << "ball in der Mitte" <<endl;
p3dxptr->comInt(ArCommands::VEL, 75);
reached();
cout << "Rückgabe Wert 1" << endl;
return 1;
}else {
p3dxptr->comInt(ArCommands::VEL, 75);
p3dxptr->comInt(ArCommands::RVEL, 0);
cout << "Rückgabe Wert 0" << endl;
return 0;
}
}
if(obj < 0.5) {
return 0;
}
}
void GPSMapTools::endForbiddenLine()
{
if(!myForbiddenLineStarted) return;
if(!checkGPS("ending forbidden line")) return;
if(!checkMap("ending forbidden line")) return;
ArPose endpos = getCurrentPosFromGPS();
ArLog::log(ArLog::Normal, "GPSMapTools: Ended forbidden line at %f, %f", endpos.getX(), endpos.getY());
ArMapObject* obj = new ArMapObject("ForbiddenLine", myForbiddenLineStart, "", "ICON", "", true, myForbiddenLineStart, endpos);
myMap->lock();
myMap->getMapObjects()->push_back(obj);
myMap->writeFile(myMap->getFileName(), true);
myMap->unlock();
myForbiddenLineStarted = false;
reloadMapFile();
}
/**
Get the closest reading in the current buffer within a rectangular region
defined by two points (opposite corners of the rectangle).
@param x1 the x coordinate of one of the rectangle points
@param y1 the y coordinate of one of the rectangle points
@param x2 the x coordinate of the other rectangle point
@param y2 the y coordinate of the other rectangle point
@param pose a pointer to an ArPose object in which to store the location of
the closest position
@return The range to the reading from the device, or a value >= maxRange if
no reading was found in the box.
*/
AREXPORT double ArRangeDevice::currentReadingBox(double x1, double y1,
double x2, double y2,
ArPose *pose) const
{
ArPose robotPose;
if (myRobot != NULL)
robotPose = myRobot->getPose();
else
{
ArLog::log(ArLog::Normal, "ArRangeDevice %s: NULL robot, won't get reading box correctly", getName());
robotPose.setPose(0, 0);
}
return myCurrentBuffer.getClosestBox(x1, y1, x2, y2, robotPose,
myMaxRange, pose);
}
/**
@param pose1 transform this into pose2
@param pose2 transform pose1 into this
*/
AREXPORT void ArTransform::setTransform(ArPose pose1, ArPose pose2)
{
myTh = ArMath::subAngle(pose2.getTh(), pose1.getTh());
myCos = ArMath::cos(-myTh);
mySin = ArMath::sin(-myTh);
myX = pose2.getX() - (myCos * pose1.getX() + mySin * pose1.getY());
myY = pose2.getY() - (myCos * pose1.getY() - mySin * pose1.getX());
}
/*-------------------------------------------------------------
getOdometry
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometry(poses::CPose2D &out_odom)
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
ArPose pose = THE_ROBOT->getEncoderPose();
out_odom.x( pose.getX() * 0.001 );
out_odom.y( pose.getY() * 0.001 );
out_odom.phi( DEG2RAD( pose.getTh() ) );
THE_ROBOT->unlock();
#else
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
virtual ArActionDesired *fire (ArActionDesired currentDesired)
{
Bottle OdoBottle;
if(Mycopyofmodule->GetBottleData("STARin",&OdoBottle,SamgarModule::NoStep))
{
ArPose MyPose;
MyPose.setX (OdoBottle.get(0).asDouble());
MyPose.setY (OdoBottle.get(1).asDouble());
MyPose.setTh(OdoBottle.get(2).asDouble());
myRobot->moveTo(MyPose,true);//update in mm
BeenCorrectedByStar=true;
}
return &myDesired;
}
/**
* The closest reading in this range device's cumulative buffer
* within a polar region or "slice" defined by the given
* angle range is returned. Optionally return the specific angle of the found reading as
* well. The region searched is the region between a starting angle, sweeping
* counter-clockwise to the ending angle (0 is straight ahead of the device,
* -90 to the right, 90 to the left). Note that there is a difference between
* the region (0, 10) and (10, 0). (0, 10) is a 10-degree span near the front
* of the device, while (10, 0) is a 350 degree span covering the sides and
* rear. Similarly, (-60, -30) covers 30 degrees on the right hand side, while
* (-30, -60) covers 330 degrees.
* In other words, if you want the smallest
* section between the two angles, ensure than startAngle < endAngle.
@param startAngle where to start the slice
@param endAngle where to end the slice, going counterclockwise from startAngle
@param angle if given, a pointer to a value in which to put the specific angle to the found reading
@return the range to the obstacle (a value >= the maximum range indicates that no reading was detected in the specified region)
Example:
@image html figures/ArRangeDevice_currentReadingPolar.png This figure illustrates an example range device and the meanings of arguments and return value.
*/
AREXPORT double ArRangeDevice::cumulativeReadingPolar(double startAngle,
double endAngle,
double *angle) const
{
ArPose pose;
if (myRobot != NULL)
pose = myRobot->getPose();
else
{
ArLog::log(ArLog::Normal, "ArRangeDevice %s: NULL robot, won't get polar reading correctly", getName());
pose.setPose(0, 0);
}
return myCumulativeBuffer.getClosestPolar(startAngle, endAngle,
pose,
myMaxRange,
angle);
}
/*!
* This is the called when path to goal reached.
*
* @param goal: Goal it was assigned.
*/
void
Advanced::goalDone(ArPose goal)
{
static int goalCount = 0;
printf("goalDone:Main: %5.2f %5.2f %5.2f\07\n",
goal.getX(), goal.getY(), goal.getTh());
if(roundRobinFlag){
if(myLocaTask->getInitializedFlag()){
if(myGoalList.size()>0){
ArMapObject* front = myGoalList.front();
ArPose top = front->getPose();
bool headingFlag = false;
if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
headingFlag = true;
else
headingFlag = false;
printf("Moving to next goal:%s %5.2f %5.2f %5.2f: %d poses %d Done\n",
front->getName(),
top.getX(), top.getY(), top.getTh(),
myGoalList.size(), goalCount++);
myGoalList.pop_front();
myGoalList.push_back(front);
myPathPlanningTask->pathPlanToPose(top, headingFlag);
}
}
}else{
printf("Localize the robot first\n");
}
}
/*!
* Gets the list of home poses.
*
* @param mapdata: Pointer to the map class.
*
* @return list of home objects.
*
*/
void
Advanced::getAllRobotHomes(ArMap* ariamap)
{
myHomeList.clear();
std::list<ArMapObject *>::iterator objIt;
ArMapObject* obj;
int i=0;
for (objIt = ariamap->getMapObjects()->begin();
objIt != ariamap->getMapObjects()->end();
objIt++)
{
//
// Get the forbidden lines and fill the occupancy grid there.
//
obj = (*objIt);
if (strcasecmp(obj->getType(), "RobotHome") == 0)
{
myHomeList.push_back(obj);
ArPose pose = obj->getPose();
printf("RobotHome[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
pose.getX(), pose.getY(), pose.getTh());
}
if (strcasecmp(obj->getType(), "Home") == 0)
{
myHomeList.push_back(obj);
ArPose pose = obj->getPose();
printf("Home[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
pose.getX(), pose.getY(), pose.getTh());
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs != 1)
mexErrMsgIdAndTxt( "arnetc:arnetc_robot_update_handler:minrhs", "Incorrect number of input arguments. Must provide robot update handler object reference returned by arnetc_new_robot_update_handler.");
uint64_t *p = (uint64_t*)mxGetData(prhs[0]);
ArClientHandlerRobotUpdate *updateHandler = (ArClientHandlerRobotUpdate*) *p;
if(nlhs < 1 || nlhs > 3)
mexErrMsgIdAndTxt("arnetc:arnetc_robot_update_handler:minlhs", "Incorrect number of output arguments. Must assign to a vector [x y theta] or to two scalars (x, y) or to three scalars (x, y, theta).");
updateHandler->lock();
ArPose pose = updateHandler->getPose();
updateHandler->unlock();
if(nlhs == 1)
{
plhs[0] = mxCreateDoubleMatrix(1, 3, mxREAL);
double* p = mxGetPr(plhs[0]);
p[0] = pose.getX();
p[1] = pose.getY();
p[2] = pose.getTh();
return;
}
if(nlhs >= 2)
{
plhs[0] = mxCreateDoubleScalar(pose.getX());
plhs[1] = mxCreateDoubleScalar(pose.getY());
if(nlhs > 2)
plhs[2] = mxCreateDoubleScalar(pose.getTh());
}
}
void GPSMapTools::endObstacleLine()
{
// TODO split into placing obstacle line on left or right side of robot
if(!myObstacleLineStarted) return;
if(!checkGPS("ending obstacle line")) return;
if(!checkMap("ending obstacle line")) return;
ArPose endpos = getCurrentPosFromGPS();
ArLog::log(ArLog::Normal, "GPSMapTools: Ended obstacle line at %f, %f", endpos.getX(), endpos.getY());
ArLineSegment line(myObstacleLineStart, endpos);
myMap->lock();
std::vector<ArLineSegment>* lines = myMap->getLines();
lines->push_back(line);
myMap->setLines(lines);
myMap->writeFile(myMap->getFileName(), true);
myMap->unlock();
myObstacleLineStarted = false;
reloadMapFile();
}
ArPose fn1(void)
{
static ArPose pose;
pose.setX(pose.getX() + 1);
pose.setY(pose.getY() + 1);
pose.setTh(pose.getTh() - 1);
return pose;
}
/*!
* Callback function for the h key. Gets the robot to home pose.
*/
void
hkeyCB(void)
{
roundRobinFlag = false;
if(advancedptr->myLocaTask->getInitializedFlag()){
if(advancedptr->myHomeList.size()>0){
ArMapObject* front = advancedptr->myHomeList.front();
ArPose top = front->getPose();
printf("Homing to %s %5.2f %5.2f %5.2f\n",
front->getName(),
top.getX(), top.getY(), top.getTh());
bool headingFlag = true;
advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
}
}else{
printf("Localize the robot first\n");
}
}
AREXPORT void ArServerHandlerCamera::cameraModeLookAtPoint(ArPose pose,
bool controlZoom)
{
myModeMutex.lock();
if (myCameraMode != CAMERA_MODE_LOOK_AT_POINT ||
myLookAtPoint.findDistanceTo(pose) > 1)
{
ArLog::log(ArLog::Normal, "Looking at point %.0f %.0f",
pose.getX(), pose.getY());
if (myCameraMode != CAMERA_MODE_LOOK_AT_POINT && controlZoom)
myPointResetZoom = false;
myCameraMode = CAMERA_MODE_LOOK_AT_POINT;
myLookAtPoint = pose;
buildModePacket();
myServer->broadcastPacketTcp(
&myCameraModePacket,
myCommandToPacketNameMap[
ArCameraCommands::CAMERA_MODE_UPDATED].c_str());
}
myModeMutex.unlock();
}