bool RobotNavigator::setCurrentPosition() { StampedTransform transform; try { mTfListener.lookupTransform(mMapFrame, mRobotFrame, Time(0), transform); }catch(TransformException ex) { ROS_ERROR("Could not get robot position: %s", ex.what()); return false; } double world_x = transform.getOrigin().x(); double world_y = transform.getOrigin().y(); double world_theta = getYaw(transform.getRotation()); unsigned int current_x = (world_x - mCurrentMap.getOriginX()) / mCurrentMap.getResolution(); unsigned int current_y = (world_y - mCurrentMap.getOriginY()) / mCurrentMap.getResolution(); unsigned int i; if(!mCurrentMap.getIndex(current_x, current_y, i)) { if(mHasNewMap || !getMap() || !mCurrentMap.getIndex(current_x, current_y, i)) { ROS_ERROR("Is the robot out of the map?"); return false; } } mStartPoint = i; mCurrentDirection = world_theta; mCurrentPositionX = world_x; mCurrentPositionY = world_y; return true; }
/** Constructor. * @param data initial stamped transform data * @param frame_id parent frame ID * @param child_frame_id child frame ID */ TransformStorage::TransformStorage(const StampedTransform& data, CompactFrameID frame_id, CompactFrameID child_frame_id) : rotation(data.getRotation()) , translation(data.getOrigin()) , stamp(data.stamp) , frame_id(frame_id) , child_frame_id(child_frame_id) { }