/* LICENSE: ========================================================================= CMPack'03 Source Code Release for OPEN-R SDK v1.0 Copyright (C) 2003 Multirobot Lab [Project Head: Manuela Veloso] School of Computer Science, Carnegie Mellon University All rights reserved. ========================================================================= */ #include #include "../headers/system_config.h" #include "../headers/CircBufPacket.h" #include "../headers/Util.h" #include "../Main/globals.h" #include "GLocalization.h" /* init() * Initialize Locmap[] which stores the locations of the markers on the * field. This only needs to be called once. */ void GLocalizer::init() { mzero(*this); /* DEBUG = true; */ DEBUG = false; /* Load in and check number of landmarks */ loadMap(); NUM_MARKERS = VisionInterface:: NUM_MARKERS; if(USE_GOALS){ NUM_LANDMARKS = VisionInterface::NUM_MARKERS + 6; }else{ NUM_LANDMARKS = VisionInterface::NUM_MARKERS; } /* Set up localization timers */ localizeInterval = SecToTime(LOCALIZATION_INTERVAL); lastMotionUpdateTime = 0UL; for(int i=0; ibody.neck_offset; if(DEBUG){ pprintf(TextOutputStream,"Starting update Motion:\n"); pprintf(TextOutputStream, "(x,y,t) (%g,%g,%g)\n(vx,vy,vt,pxy,pxt,pyt) (%g,%g,%g,%g,%g,%g)\n", position.mean.x,position.mean.y,heading.mean.angle(), sq(position.sx),sq(position.sy),sq(heading.sMaj), position.psxsy,psxst,psyst); } double delta_time = TimeToSec(move->timestamp - lastMotionUpdateTime); /* New heading is previous heading plus differential from move */ double delta_t = move->heading_delta.angle(); double new_theta = heading.mean.angle() + delta_t; vector2d new_heading = vector2d(cos(new_theta),sin(new_theta)); /* Update heading standard deviation. * Small amounts of error are added(based on elapsed time in sec) * if the robot is moving even if heading doesn't change. */ double t_var; if((move->pos_delta.x !=0.0 || move->pos_delta.y !=0.0) && (delta_t == 0.0)){ t_var = MOVE_T_VAR_PER_SEC * delta_time; }else{ t_var = MOVE_T_VAR_PER_RAD * fabs(delta_t); } double new_t_var = sq(heading.sMaj) + t_var; /* Now compute the position part of the update. */ /* Retrieve the movement in the local frame and rotate by heading */ vector2d delta_local = move->pos_delta; vector2d delta_global = delta_local.rotate(heading.mean.angle()); /* New position is previous position plus differential from move */ vector2d new_pos = vector2d(position.mean.x + delta_global.x, position.mean.y + delta_global.y); /* Check to see if values are in bounds. Stop at boundary. */ if(FIELD_BOUNDED){ new_pos = projectToField(new_pos); } /* x_var and y_var are percentage of distance if non-zero, * and 0 if the robot is not moving at all. */ double x_var, y_var; if(delta_local.length() != 0.0){ x_var = MOVE_X_VAR_PER_MM * delta_local.length(); y_var = MOVE_Y_VAR_PER_MM * delta_local.length(); } else if(delta_t != 0.0){ x_var = MOVE_X_VAR_PER_SEC * delta_time; y_var = MOVE_Y_VAR_PER_SEC * delta_time; }else{ x_var = 0.0; y_var = 0.0; } if(DEBUG) pprintf(TextOutputStream, "Deltas Local: (%g,%g,%g) (%g,%g,%g)\n", delta_local.x,delta_local.y,delta_t, x_var,y_var,t_var); double new_x_var, new_y_var, new_psxsy, new_psxst, new_psyst; double c = cos(heading.mean.angle()); double s = sin(heading.mean.angle()); double T = -delta_local.x*s - delta_local.y*c; double U = delta_local.x*c - delta_local.y*s; new_x_var = x_var*sq(c) + y_var*sq(s) + sq(position.sx) + 2*T*psxst + sq(heading.sMaj)*sq(T); new_y_var = x_var*sq(s) + y_var*sq(c) + sq(position.sy) + 2*U*psyst + sq(heading.sMaj)*sq(U); new_psxsy = c*s*(x_var-y_var) + position.psxsy + T*psyst + U*psxst + T*U*sq(heading.sMaj); if(FULL_MODEL){ new_psxst = psxst + T*sq(heading.sMaj); new_psyst = psyst + U*sq(heading.sMaj); }else{ new_psxst = 0.0; new_psyst = 0.0; if(MINI_MODEL){ new_psxsy = 0.0; } } if(DEBUG){ pprintf(TextOutputStream, "Odom (x,y,t): (%g,%g,%g)\nC: (%g,%g,%g,%g,%g,%g)\nP:(%g,%g,%g)\n", new_pos.x,new_pos.y,new_heading.angle(), new_x_var,new_y_var,new_t_var, new_psxsy,new_psxst,new_psyst, (new_psxsy/(sqrt(new_x_var*new_y_var))), (new_psxst/(sqrt(new_x_var*new_t_var))), (new_psyst/(sqrt(new_y_var*new_t_var)))); } /* Store the new results in the position and heading variables. */ position = Gaussian2(move->timestamp, new_pos, sqrt(new_x_var), sqrt(new_y_var), new_psxsy, true); psxst = new_psxst; psyst = new_psyst; heading = Gaussian2(move->timestamp, new_heading, sqrt(new_t_var), 0.0, 0.0, false); lastMotionUpdateTime = move->timestamp; } /* End updateMotion() */ /* updateSensors() * This function makes the update to pose based on sensor inputs. * This can only be called after a call to updateMotion. * This and subfunctions compute based on a reference position, * which is set to body position or neck position depending on * NECK_CONVERT flag setting. */ bool GLocalizer::updateSensors(const VisionInterface::ObjectInfo *obj_info) { /* If converting to account for neck-body offset, set reference position * to the converted position, otherwise set to position. */ if(NECK_CONVERT){ positionReference = bodyToNeck(position); }else{ positionReference = position; } /* Compute updates using reference position (neck or body) */ bool result_position = updatePosition(obj_info); bool result_heading = updateHeading(obj_info); bool result = (result_heading || result_position); /* If using neck conversion, convert back to body frame. * Then, set the position to the reference position. */ if(NECK_CONVERT){ positionReference = neckToBody(positionReference); } position = positionReference; lastMarkerTime = position.time; return result; } /* End updateSensors() */ /* updateHeading() * This updates the heading, using the marker landmarks. */ bool GLocalizer::updateHeading(const VisionInterface::ObjectInfo *obj_info) { bool value = false; /* new_* keeps track of complete update (over all sensors) to date. * current_* is the current sensor's value. * Setting vMaj to -1 is a flag to indicate intial state */ vector2d new_mean; vector2d current_mean; double new_vMaj = -1.0; double current_vMaj; double visualAngle; double t_vMaj = sq(heading.sMaj); for(int i=0; imarker[i], LocMap[i], i)) && (markerHeadTimer[i] > localizeInterval)){ /* Get the bearing to the landmark (visual angle) */ visualAngle = atan2(obj_info->marker[i].loc.y, obj_info->marker[i].loc.x); /* Heading is the global angle to the landmark * minus the visual angle to the landmark. */ current_mean = vector2d((LocMap[i].x-positionReference.mean.x), (LocMap[i].y-positionReference.mean.y)); double angleDiff = angleDifference(visualAngle,current_mean.angle()); current_mean = vector2d(cos(angleDiff),sin(angleDiff)); /* Variance is sum of measurement variance and previous variance. */ current_vMaj = VISION_ANGLE_VAR; /* Reset the timer for this marker for heading. */ markerHeadTimer[i] = 0; /* Combine the new sensor result with the previous result * For 1-d gaussians, the new mean is cross-weighted average. * (w1*t2+w2*t1)/(w1+w2) * Uncertainty is the product over the sum of variances. */ if(new_vMaj == -1.0){ new_vMaj = current_vMaj; new_mean = current_mean; }else{ new_mean = weightedAverageAngle(new_mean,current_vMaj, current_mean,new_vMaj); new_vMaj = (new_vMaj*current_vMaj)/(new_vMaj+current_vMaj); } } /* end valid landmark true*/ else{ /* If we didn't update with this marker, add to the timer. * lastMarkerTime is reset in updateSensors. */ markerHeadTimer[i] += heading.time - lastMarkerTime; } /* end valid landmark false */ } /* end for each marker */ /* If we made an update (sMaj ~= -1), check for teleportation and * combine with previous. Return true. * Otherwise, return false and don't change heading. */ if(new_vMaj != -1.0){ vector2d result_mean = weightedAverageAngle(new_mean,t_vMaj, heading.mean,new_vMaj); double result_vMaj = (new_vMaj*t_vMaj)/(new_vMaj+t_vMaj); value = true; if(CHECK_TELEPORT){ double jump_t = fabs(angleDifference(heading.mean.angle(), new_mean.angle())); if(jump_t > MAX_HEADING_STDS*max(heading.sMaj,sqrt(new_vMaj))){ result_vMaj = sq(MAX_HEADING_SIGMA); } } /* end Check teleport */ heading = Gaussian2(heading.time,result_mean,sqrt(result_vMaj), 0.0,0.0,false); } /* end if updated */ return(value); } /* updatePosition() * This updates the position based on the sensor readings on markers. * Called by updateSensors to update position. * Returns true if the position was actually updated. */ bool GLocalizer::updatePosition(const VisionInterface::ObjectInfo *obj_info) { bool updated = false; Gaussian2 new_position = Gaussian2(); Gaussian2 current_position; /* Iterate over all 6 markers. Use confidence, edge and distance * to ignore markers that are not seen or not seen well */ for(int i = 0; i < NUM_LANDMARKS; i++){ if((validLandmark(obj_info->marker[i], LocMap[i], i)) && (markerPosTimer[i] > localizeInterval)){ /* Reset the timer for this marker for position, since valid */ markerPosTimer[i] = 0; /* For each type of measurement on this landmark * get a position estimate and multiply it in. */ if(USE_RANGE && i < NUM_MARKERS){ /* Allow updates from range */ current_position = updatePositionRange(obj_info->marker[i].distance, LocMap[i]); if(!updated){ updated = true; new_position = current_position; }else{ new_position = G2Multiply(new_position,current_position); } if(DEBUG) pprintf(TextOutputStream, "Range update: (%g,%g,%g) (%g,%g,%g,%g,%g,%g)\n", new_position.mean.x,new_position.mean.y,heading.mean.angle(), sq(new_position.sx),sq(new_position.sy),sq(heading.sMaj), new_position.psxsy,psxst,psyst); } /* End use range */ if(USE_BEARING){ /* Allow updates from bearing. */ vector2d bearing = vector2d(obj_info->marker[i].loc.x, obj_info->marker[i].loc.y); current_position = updatePositionBearing(bearing.angle(),LocMap[i]); /* if(new_position.sMaj == -1.0){ new_position = current_position; } else { new_position = G2Multiply(new_position,current_position); } */ } /* End use bearing */ }else{ /* End if valid landmark true */ markerPosTimer[i] += position.time - lastMarkerTime; } /* End if valid landmark false */ } /* End for each marker */ /* If we updated, check for teleportation and combine with * previous values. */ if(updated){ Gaussian2 result_position = G2Multiply(positionReference,new_position); if(CHECK_TELEPORT){ double jump_x = fabs(positionReference.mean.x-new_position.mean.x); double jump_y = fabs(positionReference.mean.y-new_position.mean.y); if((jump_x>MAX_POSITION_STDS*max(position.sx,new_position.sx)) || (jump_y>MAX_POSITION_STDS*max(position.sy,new_position.sy))){ result_position.setsxsy(MAX_POSITION_SIGMA,MAX_POSITION_SIGMA,0.0); } } /* End check teleport */ positionReference = result_position; } /* End if updated */ return updated; } /* End updatePosition() */ /* updatePositionRange() * Computes an updated position if landmarks are visible * and within range. This uses distance to landmark and places * robot on closest point on that circle * Assumes landmark has been checked for visibility and validity. * Always makes an update. * Returns the position estimate */ Gaussian2 GLocalizer::updatePositionRange(double measured_distance, vector2d landmark_position) { vector2d mean; double sMaj = 0.0; double sMin = 0.0; double thetaMaj = 0.0; /* Expected distance to landmark based on position estimate */ double d = GVector::distance(landmark_position,positionReference.mean); /* Closest point on circle is landmark position plus * fraction of distance to odom position measured r/real r. */ mean = (positionReference.mean - landmark_position)*measured_distance/d + landmark_position; /* Center a gaussian at this location with axis * tangent to circle (perpendicular to measurement axis) */ sMin = d * VISION_RANGE_STD_PERCENT; if(CIRCULAR_DISTS){ /* If using circular distributions, use the error on range * for both deviations. */ sMaj = sMin; }else{ /* The std is 1/16th of the circle */ sMaj = d * 2.0*M_PI/16; thetaMaj = heading.mean.angle() + M_PI/2; if(thetaMaj > M_PI) { thetaMaj -= 2*M_PI; } } if(DEBUG){ Gaussian2 new_position = Gaussian2(position.time,mean,sMaj, sMin,thetaMaj,false); pprintf(TextOutputStream, "Landmark at %g %g. Range expected %g, Data %g\n", landmark_position.x,landmark_position.y,d,measured_distance); pprintf(TextOutputStream, "Range pos (%g,%g) (%g,%g,%g) (%g,%g,%g)\n", new_position.mean.x,new_position.mean.y, new_position.sx,new_position.sy,new_position.psxsy, new_position.sMaj,new_position.sMin,new_position.thetaMaj); pprintf(TextOutputStream, "Actual pos (%g,%g) (%g,%g,%g) (%g,%g,%g)\n", positionReference.mean.x,positionReference.mean.y, positionReference.sx,positionReference.sy,positionReference.psxsy, positionReference.sMaj,positionReference.sMin, positionReference.thetaMaj); } /* Return the new estimate. */ return Gaussian2(position.time,mean,sMaj,sMin,thetaMaj,false); } /* End updatePositionRange() */ /* updatePositionBearing() * Computes an updated position if landmarks are visible * and within range. This uses angles to landmarks and places * robot on closest point on the line, assuming heading correct * Returns position estimate */ Gaussian2 GLocalizer::updatePositionBearing(double measured_angle, vector2d landmark_position) { vector2d mean; double sMaj = 0.0; double sMin = 0.0; double thetaMaj = 0.0; /* Slope of line constraining robot is line along which robot * gets that visual angle given current heading: * slope m = tan of angle to X axis (heading+visual) * intercept = landmark_y - landmark_x*slope */ double c = cos(heading.mean.angle() + measured_angle); double s = sin(heading.mean.angle() + measured_angle); double tempX = c*c*positionReference.mean.x + s*s*landmark_position.x + s*c*(positionReference.mean.y - landmark_position.y); double tempY = 0; if(c != 0){ tempY = (s/c)*(tempX - landmark_position.x) + landmark_position.y; }else{ tempY = positionReference.mean.y; } mean = vector2d (tempX,tempY); /* Center a gaussian at this location with axis * along the line (axis is atan(slope)) */ double d = GVector::distance(landmark_position,position.mean); sMin = sqrt(d*10.0*fabs(sin(VISION_ANGLE_VAR))); if(CIRCULAR_DISTS){ /* If using circular distributions, use error from angle * for both. */ sMaj = sMin; }else{ sMaj = sqrt(0.5); thetaMaj = heading.mean.angle() + measured_angle; } Gaussian2 new_position = Gaussian2(position.time,mean,sMaj, sMin,thetaMaj,false); if(DEBUG){ double expected_angle = atan2a(landmark_position.y - positionReference.mean.y, landmark_position.x - positionReference.mean.x); pprintf(TextOutputStream, "Landmark %g %g. Bearing at Range %g:\n Expect %g, Data %g\n", landmark_position.x, landmark_position.y, d,expected_angle,(measured_angle+heading.mean.angle())); pprintf(TextOutputStream, "Bearing pos (%g,%g) (%g,%g,%g) (%g,%g,%g)\n", new_position.mean.x,new_position.mean.y, new_position.sx,new_position.sy,new_position.psxsy, new_position.sMaj,new_position.sMin,new_position.thetaMaj); pprintf(TextOutputStream, "Actual pos (%g,%g) (%g,%g,%g) (%g,%g,%g)\n", positionReference.mean.x,positionReference.mean.y, positionReference.sx,positionReference.sy,positionReference.psxsy, positionReference.sMaj,positionReference.sMin, positionReference.thetaMaj); } /* Return the position estimate. */ return Gaussian2(position.time,mean,sMaj,sMin,thetaMaj,false); } /* End updatePositionBearing() */ /* getPosition() * Return the position Gaussian. */ Gaussian2 GLocalizer::getPosition() { return position; } /* getHeading() * Return the heading Gaussian. */ Gaussian2 GLocalizer::getHeading() { return heading; } /* getLocation() * Wrap both the heading and position Gaussians in a * RobotPositionInfo struct */ void GLocalizer::getLocation(RobotPositionInfo * pos) { pos->position = position; pos->heading = heading; } /* resetPose() * Update the robot pose to match a specified position and heading Gaussian. * This is useful for initializing the robot and also if the robot is * "teleported". If only one of the position or heading needs to be changed, * be sure to give the old value as an argument for the one that should * stay the same. */ void GLocalizer::resetPose(Gaussian2 position, Gaussian2 heading) { position = position; heading = heading; } /* resetDeviationUniform() * Set the robot position and heading to (0,0), but with the minimum * possible confidence */ void GLocalizer::resetDeviationUniform() { position = Gaussian2(position.time, position.mean, MAX_POSITION_SIGMA, MAX_POSITION_SIGMA, 0.0, true); heading = Gaussian2(heading.time, heading.mean, MAX_HEADING_SIGMA, 0.0, 0.0, true); } /* bodyToNeck() * Converts from body frame to neck frame. */ Gaussian2 GLocalizer::bodyToNeck(Gaussian2 p) { Gaussian2 result; result.mean = p.mean + neck_offset.rotate(heading.mean.angle()); return(result); } /* neckToBody() * Converts from neck frame to body frame. */ Gaussian2 GLocalizer::neckToBody(Gaussian2 p) { Gaussian2 result; result.mean = p.mean - neck_offset.rotate(heading.mean.angle()); return(result); } /* angleDifference() * Finds the smaller angular difference between two angles: * IE the difference from -.2 to .2 is +0.4 * but difference from .2 to -.2 is -0.4 * difference from 3.1 to -3.1 is +.08 * returns a signed double between -PI and PI */ double GLocalizer::angleDifference(double fromAngle, double toAngle) { /* Compute absolute angle difference. */ double newAngle = toAngle - fromAngle; /* Make sure it is in range -PI to PI. */ while(newAngle > M_PI) newAngle = newAngle - 2.0*M_PI; while(newAngle <= -M_PI) newAngle = newAngle + 2.0*M_PI; return(newAngle); } /* End angleDifference() */ /* weightedAverageAngle() * Computes the weighted average of two angles. * Returns a Vec2 with angle in .t from -pi to pi. */ vector2d GLocalizer::weightedAverageAngle(vector2d angle1, double weight1, vector2d angle2, double weight2) { vector2d angle; if(weight1 == weight2){ angle = vector2d(angle1.x + angle2.x, angle1.y + angle2.y); }else{ double difference = angleDifference(angle1.angle(),angle2.angle()); double temp = angle1.angle()+(weight2/(weight1+weight2))*difference; angle = vector2d(cos(temp), sin(temp)); } return(angle); } /* End weightedAverageAngle(); */ /* projectToField() * Projects a point outside the field to the closest point * on the field. */ vector2d GLocalizer::projectToField(vector2d p) { /* Project into rectangle containing field and goals. */ double temp_y = sign(p.y)*halfWidth; double temp_x = sign(p.x)*(halfLength + goalDepth); if(fabs(p.y) > halfWidth) p.y = temp_y; if(fabs(p.x) > halfLength+goalDepth) p.x = temp_x; /* Now correct for corners (outside goal, behind field wall) */ if((fabs(p.x) > halfLength) && (fabs(p.y) > goalHalfWidth)){ temp_x = sign(p.x)*halfLength; temp_y = sign(p.y)*goalHalfWidth; /* Find perpendicular distance to each wall */ double d1 = fabs(p.x-temp_x); double d2 = fabs(p.y-temp_y); /* if one wall is closer, project to it. */ /* if equidistant, project to both (corner of goal and field) */ if(d1 <= d2) p.x = temp_x; else if(d2 <= d1) p.y = temp_y; } return(p); } /* ValidLandmark() * Takes a landmark and determines if it is valid. */ bool GLocalizer::validLandmark(VisionInterface::VObject landmark_measured, vector2d landmark_map, int i) { /* Eliminate place holders for whole goal objects */ if(i==VisionInterface::YELLOW_GOAL || i==VisionInterface::CYAN_GOAL) return false; /* Eliminate markers with low confidence */ if(landmark_measured.confidence <= MINCONF) return false; /* Eliminate markers at the edge of the camera image. */ if(landmark_measured.edge) return false; /* Eliminate markers that are estimated to be too far away. */ double dist; dist = GVector::distance(position.mean,landmark_map); if(dist > MAX_MARKER_RANGE) return false; /* Taking a heading on goals that are too close causes problems with our heading. */ if((i >= NUM_MARKERS) && (hypot(landmark_measured.loc.x,landmark_measured.loc.y) < MIN_GOAL_RANGE)) return false; /* Eliminate markers that don't match where they should be */ //vector2d rel_pos; //rel_pos = (landmark_map - position.mean); //if(rel_pos.length() > 4000.0) // return false; return true; } /* loadMap() * Loads in all the landmarks to LocMap[]. */ void GLocalizer::loadMap(void) { /* This is the cyan-over-pink marker */ LocMap[VisionInterface::MARKER_COP] = vector2d( halfLength+markerOffset, halfWidth+markerOffset); /* This is the pink-over-cyan marker */ LocMap[VisionInterface::MARKER_POC] = vector2d( halfLength+markerOffset, -(halfWidth+markerOffset)); /* This is the green-over-pink marker */ LocMap[VisionInterface::MARKER_GOP] = vector2d( 0.0, halfWidth+markerOffset); /* This is the pink-over-green marker */ LocMap[VisionInterface::MARKER_POG] = vector2d( 0.0, -(halfWidth+markerOffset)); /* This is the yellow-over-pink marker */ LocMap[VisionInterface::MARKER_YOP] = vector2d( -(halfLength+markerOffset), halfWidth+markerOffset); /* This is the pink-over-yellow marker */ LocMap[VisionInterface::MARKER_POY] = vector2d( -(halfLength+markerOffset), -(halfWidth+markerOffset)); /* Place holder for whole yellow goal object */ LocMap[VisionInterface::YELLOW_GOAL] = vector2d(0.0,0.0); /* The yellow goal, left (positive) side */ LocMap[VisionInterface::YELLOW_GOAL_SIDES + LEFT_SIDE] = vector2d( -(halfLength+0.5*wallWidth), -goalHalfWidth); /* The yellow goal, right (negative) side */ LocMap[VisionInterface::YELLOW_GOAL_SIDES + RIGHT_SIDE] = vector2d( -(halfLength+0.5*wallWidth), goalHalfWidth); /* Place holder for whole cyan goal object */ LocMap[VisionInterface::CYAN_GOAL] = vector2d(0.0,0.0); /* The cyan goal, left (positive) side */ LocMap[VisionInterface::CYAN_GOAL_SIDES + LEFT_SIDE] = vector2d( halfLength+0.5*wallWidth, goalHalfWidth); /* The cyan goal, right (negative) side */ LocMap[VisionInterface::CYAN_GOAL_SIDES + RIGHT_SIDE] = vector2d( halfLength+0.5*wallWidth, -goalHalfWidth); } /* End loadMap() */ //===== GLocalizerEP implementation =====================================// std::string GLocalizerEP::name("GLocalizer"); GLocalizerEP::GLocalizerEP() { pos_info = new RobotPositionInfo; mzero(*pos_info); localizer.init(); localizer.getLocation(pos_info); } GLocalizerEP::~GLocalizerEP() { delete pos_info; } bool GLocalizerEP::initConnections() { EventManager *em; uint my_id; em = EventManager::getManager(); my_id = em->getEventProcessorId(name); mot_update_id = em->getEventProcessorId(MotionUpdateHeadSummary::name); mot_update_ep = dynamic_cast(em->listenEventProcessor(my_id,mot_update_id)); vis_id = em->getEventProcessorId(Vision::name); vis_ep = dynamic_cast(em->listenEventProcessor(my_id,vis_id)); return true; } bool GLocalizerEP::update(ulong time,const EventList *events) { const std::vector *mot_updates; bool new_out=false; //pprintf(TextOutputStream,"Updating localization at time %lu\n",time); if(events->present(mot_update_id)){ mot_updates = mot_update_ep->get(time); for(uint i=0; isize(); i++){ const Motion::MotionLocalizationUpdate *move_update; move_update = &((*mot_updates)[i]); //pprintf(TextOutputStream,"Update for movement at time %lu\n",time); localizer.updateMotion(move_update); new_out = true; } } if(events->present(vis_id)){ vis_ep->get(time); //pprintf(TextOutputStream,"Update for sensors at time %lu\n",time); localizer.updateSensors(vis_ep->object_info); new_out = true; } if(new_out){ cache_tracker.updateAvailable(time); } return new_out; } const RobotPositionInfo *GLocalizerEP::get(ulong time) { if(cache_tracker.shouldUpdate(time)){ localizer.getLocation(pos_info); cache_tracker.cacheUpdated(time); } return pos_info; } REGISTER_EVENT_PROCESSOR(GLocalizerEP,GLocalizerEP::name,GLocalizerEP::create);