Autonomous Exploration with Initial Poses Known
s-arora1987 opened this issue · comments
Before adding connectivity safe exploration, I have to get full connectivity autonomous exploration working. Therefore, I have started once again from your original code in order to find the source of "Exploration failed" error. I have modified:
- MultiMapper.cpp to include the known poses of robots and made mSelfLocalizer = NULL.
- RobotNavigator.cpp to start mGetMapActionServer for both robots.
This is the sequence I follow to start exploration: robot_0/StartMapping, robot_0/StartExploration, robot_1/StartMapping, robot_1/StartExploration. Console showed a lot of these issues:
-
Missed desired rate of 0.50Hz! Loop actually took 7.8000 seconds! [ERROR] [1553454158.546228209, 479.000000000]: Could not get robot position: Lookup would require extrapolation into the past. Requested time 468.100000000 but the earliest data is at time469.100000000, when looking up transform from frame [robot_1/base_link] to frame [robot_1/map][ WARN] [1553454158.877107073, 479.300000000]: Failed to compute odometry pose, skipping scan (Lookup would require extrapolation into the past. Requested time 468.100000000 but the earliest data is at time 469.400000000, when looking up transform from frame [robot_1/base_laser_link] to frame [robot_1/offset])
No way between robot and goal! [ WARN] [1553454067.112417528, 387.500000000]: Exploration has failed!.
In either case, the robot_/odom topic shows error status in Rviz (attached image). Following the suggestions in your other posts related to similar issues, I have:
- slowed down the robots’ max_velocity from 1.0 to 0.3,
- increased operator node.cpp loop rate from 10Hz to 25Hz, and
- decreased FREQUENCY in RobotNavigator.cpp from 5.0 Hz to 1 Hz.
After these modifications, I didn’t see many “Missed desired rate..” or “Failed to compute odometry pose,..” warnings but the issues still prevails and robots stop sometimes abruptly (when they are close to each other, narrow corridors). I see that happen more frequently when closure of mapper loop takes way too long. I have attached my code here (https://drive.google.com/file/d/1MwWqplMCzA0ilQHYUECgoeNGW8mOXLOB/view?usp=sharing) . Please help me fix this.
MultiMapper.cpp to include the known poses of robots and made mSelfLocalizer = NULL.
This should not be necessary. The mapper already has an input "initialpose", which you can use to set the initial pose and start mapping directly without localization.
Have you build with "-DCMAKE_BUILD_TYPE=Release"? The mapper is really slow in a debug build.
I tried build with -DCMAKE_BUILD_TYPE=Release. The package started working without issues for very low FREQUENCY of 1 Hz. but when I increased it beyond 1Hz, it still gives "No way between robot and goal! Exploration has failed." sometimes. I have changed initial poses to locations where there is more space to move around before robots get stuck. That seems to work at times with 1 Hz < FREQUENCY < 5Hz. whenever you get a chance, could you please try my code to find how I can improve the speed of loop closure for mapper?
From what I see in your images, the relative localization of the robots seems to be incorrect. Have you verified that the robots are correctly localized when they start exploration?
Yeah.
I have another question somewhat connected to this discussion.
mExplorationPlanner->findExplorationTarget(&mCurrentMap, mStartPoint, mGoalPoint);
I am assuming that mGoalPoint found in this call corresponds to a location w.r.t. `whatever incomplete map robots know so far' instead of location w.r.t. complete map of world. Please correct me if I am wrong. For some part of my code, I need to convert mGoalPoint to coordinates w.r.t. complete map. Is there a way to do that conversion?
Of course the robot can only operate on the map it knows so far. But the coordinate system of the map does not change during exploration, e.g. (0,0) is always the point where robot 1 started. However, due to loop-closures happening the layout of the map may change over time, so a given goal point is theoretically invalidated when a new map is received. But this cannot be recovered by some transformation.
How can I modify the code to get the true location of goal-point regardless of changes in learned map?