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)
{ }
