2d pose relative to initial pose based on odom

asked 2021-09-01 11:08:31 -0500

sudo-Boris
1 ●4 ●6 ●7

updated 2021-09-02 02:59:46 -0500

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 :/.

edit retag flag offensive close merge delete

add a comment

Comments

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?

sudo-Boris ( 2021-09-02 05:26:43 -0500 )edit

add a comment

2d pose relative to initial pose based on odom

1 Answer

Comments

Question Tools

Stats

Related questions

2d pose relative to initial pose based on odom edit

1 Answer

Comments

Question Tools

Stats

Related questions

2d pose relative to initial pose based on odom