Using map for pose estimation [closed]

edit

Hello,

Quite a newbie question, but I can't understand the right way to do things properly. (I'm using groovy under Ubuntu 12.04).

I want to get a position and orientation estimations of my PR2 robot (simulated or real) in my node.

I have gmapping running that builds me a map and publish a transform between the frame odom_combined and map. On the other hand, I subscribed to the /robot_pose_ekf/odom_combined topic and I get frequent odometry pose estimations. I understand that I could use AMCL for localisation (which provides the amcl_pose topic) but to keep my architecture simple, I just want to use gmapping. I don't use only the odom estimation because of its drift over time.

I also "lookup for transform" between odom and map :

(...)
try
{
    nhCall_.getParam("gmapping_node/odom_frame", frameToTransform);
    listener.lookupTransform("/map", frameToTransform, ros::Time(0), transform_base);
}
(...)

But how do I use the transform (transform_base) as a pose estimation ? As it is a StampedTransform, I can't directly set it as my pose. When trying to set, field by field the Pose message with the transform data, I end up with the following warning :

[ WARN] [1432023038.082861765, 6687.377000000]: MSG to TF: Quaternion Not Properly Normalized

and I'm not sure normalizing it by hand is the best solution (the warning still appears when rotating). So can someone explain me the right way to use a transform for pose estimation ?

Thanks, Erwan