Making movebase work

[marc-hanheide]

team members @cburbridge @bfalacerda @cdondrup @Pandoro @Jailander @denisehe @TobKoer @PDuckworth

Identified issues

1. global planner is updating too slowly (tried 10Hz)
2. the local planner's parameterisation is off and uses many "guessed" values
3. we need a test definition (to have some "genetic" optimisation for the parameters)
4. observed problems
   1. turning towards obstacles
   2. stopping too late (maybe acceleration are just wrong)
   3. zig-zag paths
   4. making physical contact
   5. the robot always tries to go to the exact waypoint (should already be relaxed with the newer version)
   6. local cost map probably too small
   7. global cost map inflation could be wrong: We don't want the robot to be always in the middle of a corridor, so we don't want any gradual inflation.
   8. if the robot cannot reach a waypoint (closed door), it stays in front of the door blocking it (might be solved better on topological / execution level)
   9. robot is quite "pushy" navigating through crowds (this should already be working with human-aware nav)

Test cases

• general setup: L-shape, with 3 topo map with 3 waypoints (one intermediate)
• running via topo nav
• collect rosbags
• the obstacles are always "around the corner", 1m away from intersection

static tests

1. place robot close to obstacle (10cm away, 45, 90, 135), goals right behind, and at 45 degree angle
2. traverse narrow corridor (1m, corridor in global map)
3. traverse corridor with row of chairs (2m wide, leaving a 1m gap)
   a. all chairs on one side
   b. chairs on both sides
4. robot trapped
5. (optional: traverse a door)
6. blocking the exact intermediate waypoint (putting a chair there)
7. blocking the target waypoint (putting a wheelchair there), stopping at appropriate distance
8. chairs blocking the whole route
9. human standing next to wall in 2m corridor, with crutches sticking out, creating a gap of about 1m

dynamic tests

1. emergency stop: "jump" in front of robot (50 cm distance)
2. pass-crossing: (person walks / wheelchair drives) in front to the robot coming from the side: robot should stop
3. pass-by: (person walks / wheelchair drives) in front to the robot coming from the side: robot should stop
4. pass-crossing and pass-by with a group of people (group blocks the way for the robot), with a 10 second break of the group standing in front of robot

Thinks to do

• collect all nav params used across sites and comment your observation / characteristic (please comment on this issue)
• look at cost maps in the mon nav from deployment data and identify the situations where it failed
• implement above test cases and iterate parameters on all sites

To prevent regressions:

• implement navigation unit tests, based on ROS bag
• implement navigation unit tests, based on Morse

Further ideas:

• make it more clearly when the robot is about to move
• put warning lights on to indicate when it is about to move (complementing speech)
• check for RAM/CPU usage
• watch dogs need to be always running

[marc-hanheide]

OK, so this is the brief summary of what we planned to do. No it's time to get going on this.
Can ask representatives of the sites (maybe @bfalacerda @Pandoro @Jailander @PDuckworth and @nilsbore) to check the parameters they are using, post them (i.e. link them) from here and write one paragraph summarising your observations.

Then, we should create the setup for the test cases, e.g. have a shared topological map which we only slightly adapt to our own environments, but have the names consistent. I suggest, UOL (@Jailander, myself, @cdondrup) create a first prototype of the test setup.

Then we should have a hangout to discuss the next steps. Here's a doodle I ask you all to fill to arrange this.

Many thanks

[marc-hanheide]

forgot @hawesie... sorry

[bfalacerda]

I forgot we can't check what params we were using on Bob during the deployment because his hard drive died. I'm pretty sure we were using the current strands_movebase ones though.

However, @kunzel did some changes to the dwa params to make it follow the global path more closely. It was only added to scitos_2d_nav, because it makes navigation in simulation work much better. The differences are

• sim_time: 0.8 (was 2.1) This one was done because the dwa scoring only looks at the end of the sampled trajectories for 2 of the components of the scoring function. This made it choose paths that were two far away from what the global planner wanted:

cost =
  path_distance_bias * (distance to path from the endpoint of the trajectory in meters)
  + goal_distance_bias * (distance to local goal from the endpoint of the trajectory in meters)
  + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254))

• path_distance_bias: 1.0 (was 32.0, the default), goal_distance_bias: 0.5 (was 24.0, the default). This has to do with the same issue as above I think

Maybe @cdondrup has some insight on these params, since he has been looking at dwa?

[cdondrup]

Actually setting path_distance_bias to 1 and goal_distance_bias to 0.5 should have had the opposite effect and allow the robot to diverge from the global path a bit more. However, if the DWA only uses the params described above (I remember there was goal align and path align somewhere as well but that might have been in a different implementations of it) then this shouldn't have changed much. The ratio is key here so with 32 and 24 it should have been the same as 1.0 and 0.75... so by changing it to 1.0 and 0.5 we only just gave less weight to the goal_distance_bias.

The occdist_scale is the one responsible for obstacle avoidance and the default here should be 0.01 which means it only takes lethal obstacles into account and doesn't have effect on the other two params.

[bfalacerda]

yes, from what i understand, the short sim time is what makes a big difference here: since both those values only look at the endpoint of the simulated trajectories, if we increase their size we increase the number of trajectories that don't go along the global plan, but have an endpoint close to it

[cdondrup]

Isn't that what you want for obstacle avoidance?

[bfalacerda]

depends. if we increase the rate of the global planner it might be better to keep the sim time smaller? with this version we had the robot rotating a lot more in place, at least in simulation.

[Pandoro]

We are indeed using some custom parameters. We had our old hiwi run around and optimize the parameters in such a way that the behaviour would be 'optimal' and we have the following in our "site parameter"-file:

local_costmap:
  width: 4
  height: 4
  resolution: 0.02

  # Improves navigation in tight corridors tremendously.
  inflation_layer:
    inflation_radius: 0.5

DWAPlannerROS:
  max_vel_y: 0.0
  min_vel_y: 0.0

I can't really say what the idea was behind these changes, just that it felt a bit more stable after he played around with the parameters. In general navigation seems okayish, apart from of course the driving towards obstacles and the slightly awkward behaviour in our kitchen that I described during the GA. According to our launch file we are supposedly running the human aware navigation, but he doesn't stare at people so maybe something went going wrong there. I'll check more accurately on our next run with Karl.

[marc-hanheide]

Could you please submit a link to the "whole" set of files, as this already assumes there would be a standard you deviate from ;-)

[Pandoro]

You can check out the strands karl repo. This should have everything we use, including the above file. We took most of the stuff from what we had before in the marathon system and updated it with everything new and relevant from the review system.

[marc-hanheide]

@Pandoro @Jailander @cdondrup Can I ask you to fill the doodle poll at https://doodle.com/poll/56f7c5ydtqny8p78 as well, please?

[marc-hanheide]

For the record, I link the current versions of params here:

BHAM

• normally based on strands_movebase
• with some changes dicussed in Making movebase work #62 (comment)

RWTH

• seems pretty much standard, but this is the used version

UOL

missing

KTH

• standard parameters

[marc-hanheide]

@bfalacerda do you remember why we move to our own NavFn? 5618362

[bfalacerda]

yes, so we could reconfigure the default_tolerance param a runtime. This makes the robot not go to the exact pose of the intermediary waypoints.

[marc-hanheide]

right, I remember...

[nilsbore]

A git diff to the branch we are using reveals no changes to the parameters.

[Jailander]

@marc-hanheide in Linda we use the standard parameters namely the ones at https://github.com/strands-project/strands_movebase/tree/indigo-devel/strands_movebase/strands_movebase_params

[marc-hanheide]

simulation env in strands-project/strands_morse#138

[nilsbore]

http://answers.ros.org/question/201535/move_basedwa-parameters-for-a-large-robot-with-rotation-center-in-the-front/

Then I thought well, I am going to read the source code, which helped a lot. and thanks for good coding + comments. In dwa_local_planner page, the forward_point_distance is defined as an additional scoring point in front of the robot. I frankly didn't understand what it meant. However, in the source code, I found out that this value is being used as a cost value for robot alignment to the path. By setting it to zero, the trajectory will not be penalized if the robot is not aligned on the path. This is a very important factor for differential drive robots whose centre of rotation is not the in the middle of the robot. Apparently, a lot of valid trajectories were being dropped because of this. As soon as I set this to zero, the robot was able to perform much better. However, improvements could be done so I continued.

This makes me want to investigate how the robot behaves if we set forward_point_distance: 0.0. The point afterwards about visualizing the evaluated trajectories also sounds like a good point.

[marc-hanheide]

very useful @nilsbore !

Quick summary of today's hangout:

1. @Jailander to define the first test case as part of strands_navigation unit test as a blueprint
2. @bfalacerda to talk to @cburbridge to fork https://github.com/ros-planning/navigation into STRANDS including Chris' visualisation of policies (and release it from there)
3. @cdondrup to work on the testing framework, including recording of trajectories. It has been discussed that we split the test into a test preparation service which will differ in simulation and real, and an actual unit test that call the preparation service (e.g. to "beam" the robot to its starting position), and then run the actual test.
4. Everybody to replicate first test case "live" once working
5. Everybody to add additional unit tests for the other cases identified above (incrementally, always in line with live robot testing)
6. @marc-hanheide to facilitate simulation test on Jenkins: facilitating full simulation test in Jenkins strands_ci#15
7. @creuther to check again for relation between force and DWA parameters

[cburbridge]

I have forked navigation into strands-project and added my debug out changes to the "strands-testing" branch. These changes are not permanent as they make the code less efficient, so this branch should only be used for debugging etc. Therefore I don't think a "release" would be a good idea.

To use it, download and build the repo in you workspace then restart navigation, and run the (ugly) python script dwa_planner/scripts/log.py. This will open a window with six images, each showing the score for the critics used in the code. The axis for each graph are: y=angular velocity, x=linear velocity, centre is zero and the values are scaled by 10. so (x=10,y=30) represents (lv=0, wv=0), (x=15,y=20) represents (lv=0.5, wv=-0.1). The graphs are updated in real time as the robot drives, but some timesteps will be missed due to the speed of the redraw. If this is a problem then you can log the topic debug topic and replay it slower...

[marc-hanheide]

@cburbridge agreed, not to be released. I just thought we'll eventually fix code in navigation and then we'd have our own. But for now, let's leave it as a debug tool.

[marc-hanheide]

People will (probably) like to hear that Jenkins can now run simulation-enabled test (more precisely, it can run test in a virtual OpenGL/X environment, hence allowing us to write unit test that use the simulator and/or other GUIs. strands-project/strands_navigation#270 is a first example (though a stupid one as it only starts the simulator but doesn't do anything with it, but it's a test). One outcome of any tests we now run on jenkins (devel or PR) is now a highly compressed (1 fps) video (see https://lcas.lincoln.ac.uk/jenkins/view/Pull%20Requests/job/pr-indigo-strands_navigation/98/artifact/Xvnc.mp4 for the corresponding example) showing the virtual desktop (jump to minute 6 in said video to see the simulator appearing). This video should help to identify causes of failure in unit tests.
As this is facilitated by a virtualised compute server GPU, you cannot expect the performance of a real GPU here, but it's enough to run morse in wireframe (and maybe also small/simple environments).

Having written all this, I should probably write the same to the mailing list...

Anyway, so the infrastructure is there to actually write more elaborate tests... now somebody needs to do it ;-)

[creuther]

I talked to my people and this is the (probably not as satisfying as hoped for) gist of it:

1. The SCITOSDriver specifies a whole lot of maximum/minimum velocities and maximum positive/negative accelerations. These values are passed down to the MCU (Main Control Unit) and will be used as soft limits in the motor controller. Most of the time these constraints are met, but apparently it cannot be guaranteed (only in extreme situations).

This means that the "drive parameters" of all STRANDS robots are currently specified here.
More specifically, these parameters govern the way soft limits:

        <MaxForwardVelocity>1.0</MaxForwardVelocity>
        <MaxBackwardVelocity>-0.4</MaxBackwardVelocity>
        <MaxRotVelocity>120</MaxRotVelocity>
        <TransEpsilonVelocity>0.02</TransEpsilonVelocity>
        <RotEpsilonVelocity>1</RotEpsilonVelocity>
        <MaxTransAcceleration>0.8</MaxTransAcceleration>
        <MaxTransDeceleration>0.9</MaxTransDeceleration>
        <MaxRotAcceleration>180</MaxRotAcceleration>
        <MaxRotDeceleration>260</MaxRotDeceleration>
        <MaxEmergencyDeceleration>1.6</MaxEmergencyDeceleration>

Rotations given in degrees / {second,second^2} and translations given in meters / {second, second^2}.

The changes @bfalacerda made with respect to the DWA parameters should reflect those parameters. The parameters can also be tweaked to achieve softer acceleration, more abrupt braking, etc.

2. (paraphrasing Stefan, as I don't know a lot about electronics): The force basically imposes a threshold for the motor current, which is encoded as a 8 bit PWM (hence the values 0-255 cover the complete analogue spectrum). From a motor controlling perspective it would be ideal to always drive at full force, but due to wheel slippage and things like relying on the robot to get stopped by the charging station's threshold, we compromise and use a value of typically around 80-120. Therefore lowering the force too much will increase the stopping distance. Also, with a force of around 100 the acceleration/deceleration limits set (see above) aren't necessarily reached, so increasing the force will enable the robot to get closer to the set limits if they are "quite high".

There is no real formula or table I can give you regarding the force and the accelerations. But setting lower values for accelerations should mitigate the influence of changing the force on the acceleration/deceleration behaviour.

[marc-hanheide]

given the values are defined in the Mira config file would it make sense to expose them as rosparams and have DWA configured from those params? obviously we would expose them as rad/s(^2) where needed?

[creuther]

Certainly possible, they can be queried in MIRA and therefore exposed as rosparams. Though note that they are not dynamically reconfigurable, they are written to the MCU at startup and then can't be changed during runtime.
But still, it would probably beat having to maintain two sets of files (robot config file and dwa params file) and all the potential trouble that comes with that. I can get together with @cburbridge to help expose the properties.

[cburbridge]

I have done some test in the scitos_mira code, and I think there is a ~400ms delay between commanding a velocity and the odometry reporting that the robot is moving at that speed. This is just the delay on the MIRA side, it does not include any delay between the DWA selecting a velocity and the MIRA code sending that velocity to the firmware. @creuther would you expect this kind of timing?

[nilsbore]

I made some improvements to how the clouds are processed by movebase, which should gains us some more CPU on the main computer: #64 .

[marc-hanheide]

In response to #62 (comment): @creuther can you comment? A delay of almost half a second can indeed explain a lot of stuttering I guess. Is there a way to reduce this or is there a way to make DWA aware of such a delay (asking very silly questions)...

[marc-hanheide]

agreed in meeting today:

• @bfalacerda to make sure we only use strands_movebase parameters (use strands_movebase for all environment launch files strands_morse#144)
• put the correct params as discussed above in strands_movebase
• update the footprint
• make sure that all those PRs related to this are merged
• run the tests as described in https://github.com/strands-project/strands_navigation/blob/indigo-devel/topological_navigation/tests/README.md from a workspace that contains
  • https://github.com/strands-project/navigation
  • https://github.com/strands-project/strands_movebase
  • https://github.com/strands-project/strands_navigation

[nilsbore]

Sorry I wasn't there, had forgotten to put it in my calendar. Will follow up on this.

[nilsbore]

It seems that the moving of the laser and chest camera frames, together with the changing of the footprint, has improved navigation quite a bit in some aspects. What I've seen here is that the robot is traversing doors noticeably better than before.

Overall, when I ran navigation for a few hours the other day it seemed more robust but that is harder to pen down to something specific. Time to run the navigation tests =P.

[cdondrup]

Currently failing tests in simulation due to parameters:

• test_static_1m_l_shaped_corridor: After taking the bend the robot comes too close to one of the walls and shows the well known behaviour of just turning towards it and dying.
• test_static_70cm_wide_door: Still unsure if this will ever be solvable.
• test_static_corridor_blocked_by_humans: Robot plans through groups of humans (even though there is not enough space) and traps itself between them.
• test_static_corridor_blocked_by_wheelchairs: Robot plans through one of the wheelchairs because it only sees the wheels. This might be fixable using 3D obstacle avoidance. Nevertheless, there is not enough space for the robot to go between the wheels either.
• test_static_facing_wall_10cm_distance_0_degrees_goal_behind: Too close to wall for move base to get free.
• test_static_facing_wall_10cm_distance_minus_45_degrees_goal_behind: Too close to wall for move base to get free. Turns to +45 and then dies.
• test_static_facing_wall_10cm_distance_plus_45_degrees_goal_behind: Too close to wall for move base to get free. Turns to -45 and then dies.
• test_static_human_on_end_point: Not able to go back to start after failing to reach the end point.
• test_static_wheelchair_on_end_point: Not able to go back to start after failing to reach the end point.

[nilsbore]

Maybe it's time to switch strands_morse to strands_movebase and possibly add the chest camera to the simulation. The nice thing about the chest camera in simulation is that we can use the point cloud as-is, it won't be as expensive as the computations we do for the head cam to create the color point cloud and the images. What do you think?

[bfalacerda]

if navigation is being launched using strands_navigation.launch, then morse is also using strands_movebase. see strands-project/strands_morse#144

[bfalacerda]

regarding chest cam, we could add it yes, i guess it'll be important for the wheelchairs

[Jailander]

yes that would be brilliant, however I think jenkins can only run simulations in the fast mode without cameras (right @marc-hanheide ?) but we could still run the tests independently using a flag like for the real robot

[nilsbore]

Ok, I was just looking at this launch file: https://github.com/strands-project/strands_morse/blob/indigo-devel/mba/launch/move_base_arena_nav.launch .

[nilsbore]

I'll have a look at adding the chest cam sometime during this week.

[marc-hanheide]

Can we please leave the default without chestcam in simulation. Many students and other people (including jenkins) don't have a suitable GPU and can't run the full Morse. Of course it would be great to add chestcam and a suitable flag.

[nilsbore]

Of course, it will be an option.

[cdondrup]

Once we have a navigation set-up that runs in simulation and on the robot, I can add that to the tests and have a flag as well, yes.

[cdondrup]

We did the first real robot tests based on the test scenarios in simulation. You find a few videos here: https://lcas.lincoln.ac.uk/owncloud/index.php/s/GppNjlMSMyaHTVf#

Short summary or what isn't working:

• the robot starting 10cm away from the wall. Doesn't even move regardless of the angle it is facing it at.
• the middle waypoint blocked by an obsctacle, robot doesn't even try to move. According to @Jailander that should have been fixed and the robot should try to go as close as possible to that point at least but we didn't observe that.
• the last waypoint is blocked. The robot doesn't even try to move towards the last waypoint after reaching the intermediate one. Not sure if this should be changed or is actually a good behaviour.

[marc-hanheide]

Many thanks, @Jailander and @cdondrup for getting going on this.
I think it's now time we reconvene on hangout to replicate this at the various sites and to discuss the improvements possible to make it pass all those tests. The latter seem to be mostly related to topological navigation, so @Jailander and @bfalacerda are probably the ones to look at those.

Question to @cdondrup: I presume this was based on the latest set of parameters etc. Just for the record, can you comment and link the exact commits/released versions you have been using for your tests. Also, given you previous experience, would you say this was already better than what we had in the past, as we had some improvements (accelerations, laser position, etc) implemented already?

In any case, I put a doodle for the next meeting out: https://doodle.com/poll/mtynvcxw23fnsmdp

Please pick you slots quickly, so we can have the meeting in short term.

[bfalacerda]

i'd expect the robot to at least try to get closer to the wp in the second situation, even if he doesnt manage to get through it... I'll also try that and see what happens. Is float64 xy_goal_tolerance of the waypoints set to something?

Regarding the last one, that's the expected behaviour and I also think it's acceptable. we cant really hope to do anything better than that.

I have a suggestion regarding this intermediate waypoints issue, and also all the weird trajectories the robot takes when following a policy between nodes: the current version changes goal once we get into the influence area of a node, and the next action is also move_base/human_aware. I suggest a two step lookahead instead, i.e., if we have a waypoint sequence A->B->C->D, all with move_base, once we get to the influence area of B, we send a move_base goal to D. This will surely make the robot's movement much smoother (e.g., it'll probably avoid the weird nav in the AAF corridors), and will also make it more robust to occupied intermediate nodes. The only drawback I see is when the robot is doing B->C->D, and ends up not getting into C's influence area, for example because of an obstacle. We'd have to be careful with that during execution, and with the nav stats.

@Jailander does this sound sensible?

Another option would be to change the move_base goal immediately after the current target becomes the closest_node. That would have the same advantages and issues more or less.

[Jailander]

@bfalacerda that could be possible I'm unsure of the consequences of this in terms of making the robot follow the actual topological route and not taking another route, I'll put it in as soon as I have time on a separate branch and test it in simulation.

About the XY tolerance you are right this this should have worked, I'll rerun the tests today

EDIT: we already change the move_base goal immediately after the current target becomes the closest_node (forgot to write this)

[cburbridge]

This needs fixing in the simulation as it might have an effect on the testing results strands-project/strands_morse#143

[bfalacerda]

@Jailander when was that changed? is nav smoother now?

[Jailander]

oops no my mistake, just checked, I did that in the ICRA branch but never pushed it upstream as that branch is substantially different from our main system, the behaviour it had was worse, the reason for that is that when the robot changed goal sooner it was going faster so it stopped harder (but in that case there was a bigger time gap between move base goals because we switched maps too), I am not sure if we really should try it in the main system too, do you think its necessary?

[bfalacerda]

I think he shouldn't stop at all, if you send a new goal to move_base it usually changes quite smoothly. We have deceleration now because he was getting close to the goal before receiving the new one, but if we send new goals from further away it should be smoother. Why does he stop when he gets a new goal?

[Jailander]

hmm in that case it could have been because there was no (metric) map for some time, I'll test it on normal conditions and tell you.

[marc-hanheide]

Minutes of Meeting:

• We only use the parameters in strands_movebase
• We re-released strands_movebase, strands_navigation, scitos_common to be up to date
• Then everybody should replicate the tests according to the video¯

[marc-hanheide]

All releases are out. Time to update and run the tests.

[nilsbore]

Unfortunately, it looks like Rosie's PCB power board has fried again, meaning we have no main computer atm. We'll see what happens but it likely won't be possible to run tests this week.

[marc-hanheide]

Ah, bugger... OK, we'll have to see what we can do here and hopefully Birmingham @kunzel @bfalacerda will be able to run on their site?

[bfalacerda]

yes, we have student that's going to help, I'm meeting him today to set up the testing. Just to confirm, the idea now is to run the static tests one by one and report the result right?

[bfalacerda]

So we have the robot ready to run tests, we just need to make some arenas resembling the scenarios.

@Jailander @cdondrup the output from the "push robot to start location" always tells me I'm 2 meters away from the goal. Ignore this, the topo node pose wasn't being updated properly for some reason