Ask Your Question
0

2d pose relative to initial pose based on odom

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

sudo-Boris gravatar image

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

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-09-02 05:12:14 -0500

dornhege gravatar image

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.

edit flag offensive delete link more

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 gravatar image sudo-Boris  ( 2021-09-02 05:26:43 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

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

Seen: 16 times

Last updated: Sep 02