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_[0] + msg->pose.pose.position.x, initial_pose_[1] + 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 :/.