Handle map messages even if there is no initial pose #1130
Conversation
|
|
||
| // Instantiate the sensor objects | ||
| motion_model_.reset(); | ||
| motion_model_ = std::unique_ptr<nav2_amcl::MotionModel>(nav2_amcl::MotionModel::createMotionModel( |
There was a problem hiding this comment.
Why was this ever starting the motion model?
There was a problem hiding this comment.
Originally on every map update, pf was getting reset and all the map data was getting cleared and the motion model was getting deleted and reinitialized.
| // Laser | ||
| lasers_.clear(); | ||
|
|
||
| handleInitialPose(last_published_pose_); |
There was a problem hiding this comment.
When initialPoseReceived is called, it will call handleInitialPose. I don't see any reason on why handleMapMessage should handle the initial pose.
There was a problem hiding this comment.
It seems like in navigation1 there was some pose handling on new mapCb https://github.com/ros-planning/navigation/blob/melodic-devel/amcl/src/amcl_node.cpp#L1574
I'm not sure if related but I'm not seeing an analog here
There was a problem hiding this comment.
So far we have been setting the initial pose from rviz, but we need to also be able to handle it if it comes programmatically before the first map is ever loaded. I think we are missing this, we should open an issue to support it.
| lasers_.clear(); | ||
| lasers_update_.clear(); | ||
| frame_to_laser_.clear(); |
There was a problem hiding this comment.
I dont think these are at all related to the maps. This would be a huge pain when you have to do map sharding and you're constantly loading new section of the map in a large building. I think these should go back to where they were
There was a problem hiding this comment.
There is this comment regarding these: // Clear queued laser objects because they hold pointers to the existing // map, #5202. in the original code.
There was a problem hiding this comment.
So why was this in another method before?
There was a problem hiding this comment.
It was in handleMapMessage method right after calling the freeMapDependentMemory. Since the only thing that freeMapDependentMemory now does is to delete the map, I decided to put it in the freeMapDependentMemory to make the handleMapMessage method cleaner.
|
I'll admit I haven't thought too much about the AMCL on map change things, but there's some stuff going on here that seems fishy to me, can you explain more about all the changes you made? |
|
I'm just going to rescind my review - I don't understand what's all being changed and way |
SteveMacenski
requested review from
SteveMacenski
and removed request for
SteveMacenski
|
@mhpanah Can this be merged now? |
Basic Info
Description of contribution in a few bullet points
first_map_only_flag is set to false, we are resetting pf every time the map callback is called. This can cause divergence. To fix this issue, every-time we get a map update, I only clear the map and laser data without re-initializing pf every time.