search

skasperski / navigation_2d

ROS nodes to navigate a mobile robot in a planar environment
GNU General Public License v3.0
123 stars 65 forks source link

Autonomous Exploration with Initial Poses Known #52

Closed s-arora1987 closed 4 years ago

s-arora1987 commented 5 years ago

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:

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:

  1. 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])

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

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.

learned_map

skasperski commented 5 years ago

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.

s-arora1987 commented 5 years ago

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?

skasperski commented 5 years ago

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?

s-arora1987 commented 5 years ago

Yeah.

s-arora1987 commented 5 years ago

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?

skasperski commented 5 years ago

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.

s-arora1987 commented 5 years ago

How can I modify the code to get the true location of goal-point regardless of changes in learned map?