# 2d pose relative to initial pose based on odom

Hi, so I want to calculate the distance of the agent to the goal. For that I constantly need its current location and I get this just by using the odometer data. And everything is fine, when I don't use the pose estimate in rviz, but once I do, the odom data isn't relative to the new initial position. At least that is what I think. So my question is, how I would calculate the real position, based on the odom data, relative to the new initial pose.

void SpacialHorizon::odomCallback(const nav_msgs::OdometryConstPtr& msg){
odom_pos_=Eigen::Vector2d(initial_pose_ + msg->pose.pose.position.x, initial_pose_ + msg->pose.pose.position.y);
}


-> initial_pose_ is set in a initial_pose callback. This is what I was thinking of, but it somehow completely messes up the position.

If I'm having a conceptual misunderstanding I would very much appreciate an explanation. I am stuck on this one at the moment :/.

edit retag close merge delete

Sort by » oldest newest most voted The initial pose is meant as an initial estimate for localization or SLAM relative to a given fixed frame (usually NOT odom), while odometry usually is a continuous relative movement of the robot. So this is not meant as a "reset odometry" function.

The cleanest/correct way would be to receive the initial pose (this is just a pose message published on a topic) and when this arrives: Record the odometry at that pose, Remember the given initial pose. And for any following odometry messages calculate the relative odometry to the odometry at the initial pose and add that relative shift to the initial pose.

more

So do you mean something like this:

initial_pose_callback(initial_pose_msg):
initial_pose = initial_pose_msg
initial_odom = current_odom_pos

odom_callback(odom_msg):
odom_pos = (odom_msg - initial_odom) + initial_pose


But in this case isn't current_odom_pos still 0, if I don't drive around in the beginning and only set a new initial pose?