Initialpose not taking into account frame_id?

[robin-zealrobotics]

There might be something wrong with my setup, but so far I've seem to narrowed down on a problem with the 'initialpose' topic.

Setup:

• Rtabmap is configured with map_frame_id=='slam_map'
• a static transform is defined from 'map' to 'slam_map'
• using rviz I publish to initialpose and inspecting the message I see it correctly publishing the pose in 'map', and with the correct coordinates in that map
• monitoring the output on localization_pose, I however see it publishing the coordinates as sent from initialpose, but in the frame 'slam_map' instead, without transforming it accordingly

Not sure if this is intended? is 'map_frame_id' used to override the output frame and is rtabmap just ignoring the actual frame in initialpose? I would assume 'map_frame_id' is intended for lookups in the tf tree and was expecting it would transform initialpose to the 'map_frame_id' before using it.

[matlabbe]

Well, it looks like we don't even check the frame_id set in initialpose, assuming it would be in same map frame set by rtabmap node:

rtabmap_ros/rtabmap_slam/src/CoreWrapper.cpp

Lines 2527 to 2537 in ef8ec43

	void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
	{
	Transform intialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
	if(intialPose.isNull())
	{
	NODELET_ERROR("Pose received is null!");
	return;
	}
	rtabmap_.setInitialPose(intialPose);
	}

To support initialpose sent in frame_id different than map_frame_id set to rtabmap, we would need a TF lookup transform to get it in map_frame_id.

A workaround in rviz would be to set temporary the global frame to slam_map, send the initial pose, then set back to map.

EDIT: It seems that AMCL doesn't support that either:
https://github.com/ros-planning/navigation/blob/9ad644198e132d0e950579a3bc72c29da46e60b0/amcl/src/amcl_node.cpp#L1550-L1557

[robin-zealrobotics]

Atm I fixed it on my end with a simple node I made that does the transformation and republishes.
I think it would be fine to not support it, but then we should at least print a warning when it’s being ignored, similar to AMCL.

The reason that it was a confusing bug for me to find is that I had an EKF that processes the pose from rtabmap, that I also reset with that initialpose. With that setup, it looked like it worked as it teleported to the spot I dropped the pose, but then over the course of a couple of seconds it started sliding to a ‘random’ pose, which in fact was that same pose, but in the other frame. So it wasn’t immediately obvious to me this was caused by frame transformations instead of e.g. problems from mapping.