update of dynamic objects in costmap using chest camera

[ToMadoRe]

Navigation with the chest camera seems to solve quite a few issues in our environment. However, one thing we have noticed is that dynamic obstacles aren't updated in the costmap, i.e. if a person is crossing in front of the robot, the robot will stop and add the person to the costmap. But then it never disappears again until the robot is moved manually. Is there any solution to this?

[nilsbore]

This sounds strange. If the camera is seeing the moving object, the costmap should be updated accordingly. However, if the camera has seen the object and it moves when the robot has turned away, the costmap will not be cleared. Can you try to make out in which of these scenarios this is happening? You can view the /move_base/points_subsampled to get an idea of what the camera is seeing. You can also add a Pointcloud(not 2) on topic voxelgrid_marked (or similar) to see the occupancy in 3d.

[ToMadoRe]

Well, it happens if you step in front of the robot. There are just obstacles added to the costmap (/move_base/local_costmap/) but not removed. It just deletes these obstacles from the map when they are behind the robot. /voxel_marked_cloud shows me the points in 3D and there obstacles don't get removed either.

[nilsbore]

This is really strange, I will have to have a look at that. This part has been working nicely on our robot. Can you please check that /points_subsampled is aligned well with the floor in rviz?

[nilsbore]

Another thing that would be good to check if you have any printouts from scitos_2d_nav, like "can not raytrace because camera is outside of map". If the camera is too high up (I think above ~1.2 m) this will happen.

[nilsbore]

I've been testing this again and I can't get it to fail. If you want to, we can debug over skype, my username is nils.bore.

[ToMadoRe]

I will try to point the camera a bit "steeper" and re-calibrate it... Have to leave soon but maybe I will get back to you next week then. Cheers

[nilsbore]

It shouldn't really matter at what angle you put it. Just make sure that the pointcloud looks like it's aligned in rviz. Sure, have a nice weekend!

[ToMadoRe]

@nilsbore : Can we maybe debug over Skype later on? I still have the same problems even after re-calibration and improving the camera mounting.

[nilsbore]

Yeah, sure. How about now?

[nilsbore]

So we tried to debug this now and got it to work by using just the chest camera by setting with_camera:=false when launching scitos_bringup. I have a hypothesis of what went wrong here. It might have been that the chest camera updates were clogged for some time, obstacles were added but not removed since no new frames were coming in. So therefore it worked with just one camera, because the images didn't get clogged then.

What we did at KTH to make sure that the chest camera was always coming in at a good framerate was to connect that to one of the usb 2 ports while connecting the head camera to the usb 3 port(and swap the sound or touch cable to a usb 2 port to get to that port).

[ToMadoRe]

it seems to work now. As Nils said, the problem was probably due to using both cameras.

[ghost]

Our problem is similar to this I think. We attached the camera, ran the calibration and made sure both cameras give nice streams. If you visualize the stream it comes in a good framerate, but most objects we put in front of it aren't added to the costmap. If we put something on the ground, which is below the range of the laser scanner it simply ignores is and pushes it away. Same goes for feet. Is there some threshold to how big an object must be before it is added?

[nilsbore]

So it does add larger objects and clears them correctly? Currently it only adds objects that are more than 1 dm above ground. You can try to change this as detailed in https://github.com/strands-project/scitos_2d_navigation/issues/21.

[nilsbore]

Also, it's good to have a look in rviz to see that the cloud looks like it's well aligned to the floor after "calibration".

[ghost]

The calibration actually looks very good. However for us it seems to work with objects around 2dm in size. For the rest it does seem to work okay.

What are the further plans? I can understand that recognizing feet is rather hard as this could easily be noise, but wasn't this one of the reasons why we also wanted this chest camera ?

[ToMadoRe]

We also had this update problem again yesterday. Once I ran across in front of the robot where it stopped accordingly. But then it never continued until I pushed the robot a bit. Sometimes the costmap just does not remove obstacles when the disappear.

[nilsbore]

Ok, can you try to replicate this and save the costmap or pin down where the lingering obstacles were in the field of view? Did you get a chance to see if there was anything lingering in the costmap? I think I will have to make some kind of setup with my camera on the desk to debug this (robot in Germany atm).

[nilsbore]

@AlexanderHermans As you say it is very hard to separate e.g. feet from the floor because of noise. I can't really say how we would solve this. What could be useful would be to drive around and map the maximum height above the floor measured for different areas of the field of view and accumulate them in a discretized map. One could then go about making a threshold that would vary across the floor (noise is heavily dependent on distance). One problem there is that everyone will have slightly different angles of their cameras and so you might have to do it for a larger portion than the FOV.

I'll try to do something like that when the robot gets back. In the meanwhile, did you have anything in mind for the marathon that could pose a problem?

[ghost]

@nilsbore not really. I was just wondering what the point of the whole thing is if it barely detects something (Not to discredit your work here! :x) But maybe it is just that most of the obstacles in our lab are detected by the laser anyway.

For the marathon I am rather scared about the fact that on an average day Karl gets stuck at least once ...

[nilsbore]

The problem that we needed to solve was that it ran into obstacles above or slightly below the laser, chairs with wheels, tables and the dishwasher door when it was open. We couldn't avoid those without the camera. So I think this was an important addition. Then of course it would be good if it could avoid small objects on the floor and we should try to get that better, it's certainly possible.

Do you think that the getting stuck has something to do with the front being flat as described in this issue: https://github.com/strands-project/scitos_2d_navigation/issues/22?

[ghost]

Yea, the dishwasher seemed to be detected, but apart from that I haven't seen a lot of improvements, but since yesterday it has had several problems, so I haven't really been able to see actual improvements yet.

[ghost]

@nilsbore Sorry I somehow missed your reference to #22

It could be yes, however we have seen some strange behavior that is not based on this, most notable:

• It stood somewhere in a room spinning in circles without end.
• It just stood in front of the charging station doing nothing.

Do you know how to change the footprint? If so I can see if this improves the lifetime of an average run in our lab.

[bfalacerda]

@AlexanderHermans , move_base parameters are here:

https://github.com/strands-project/scitos_2d_navigation/tree/master/scitos_move_base_params

The foot print is on costmap_common_params.yaml

Did you chec what the robot was trying to to in those failure cases you mentioned?

[nilsbore]

You can try to change the footprint in scitos_move_base_params_3d/costmap_common_params.yaml. I think the uncommented footprint is corresponding more to the outline of the robot. But I think it could be effective to just have a half circle or triangle at the front to make sure it does not get stuck with the front against a wall.

As for the other issues, it's hard to say. It clearly didn't find a path so something was probably wrong with the costmap if the route was clear. We noticed that there could be problems relating to the narrow field of view. For example, if it is going to left or right but has to go straight ahead first for a bit, the obstacle can be seen first but disappearing when it has gone forward a bit. Then it doesn't bother to turn around to see if the obstacle is gone because it thinks it can't go there. For us this happened sometimes when it wanted to go into rooms and someone was standing in the doorway. But then it would skip those rooms since it couldn't plan a path there.

[ghost]

I will try this now. The second one indeed looks very similar to the actual footprint.

Sadly I didn't check what it really was doing exactly.

When it was standing in the other room, rotating, it looked a lot like the behavior it shows when it is looking for the charging station (as it was rotating very slowly). I reconfigured the battery parameters just before, so I think it was mis-localized, although I have no idea how this happened as it drove there correctly. The second time it wasn't really doing anything anymore, neither was it printing any error messages if I recall correctly, so maybe something just died.

I will report back, but unless it starts going wrong DIRECTLY, it will be hard to say if the different footprint was really better. Do you know why it was changed into a box at all?

[bfalacerda]

yes, the one that it there is too detailed and makes move_base too slow.

My idea was keeping a rectangular shape, but increasing the front,so it becomes symmetric

[ghost]

It seems to be running fine currently :x Although it does indeed complain that the control loop misses its desired rate of 10Hz.

On Fri, Nov 15, 2013 at 4:28 PM, BFALacerda notifications@github.comwrote:

yes, the one that it there is too detailed and makes move_base too slow.

My idea was keeping a rectangular shape, but increasing the front,so it becomes symmetric

— Reply to this email directly or view it on GitHubhttps://github.com/strands-project/scitos_2d_navigation/issues/20#issuecomment-28576902 .

[ghost]

Okay, so we tried making it fatter in all directions, including the front, that was pretty... let's just say it didn't work. @BFALacerda, your suggestion isn't working extremely well either, while it works better than our first idea, it gets stuck pretty often still. The best we saw was actually the real footprint, but this is indeed getting slower with time. So I am trying to approximate this now with 8 points instead of the original 30 points of the actual footprint. This seems to be running okay at a first glance, but more detailed experiments on Monday. Sadly I don't dare to let it run over the weekend as I fear it might not be able to charge and I don't want to know what that does to the battery :x

[nilsbore]

@ToMadoRe , @AlexanderHermans So if you still have the problem of currently seen obstacles not being removed, it could be that you're camera is looking more up than ours. The problem could then be that the ray-tracing range is currently set to 2.8 m. You can try the change in my repo https://github.com/nilsbore/scitos_2d_navigation or manually set the raytrace_range of clear_sensor to 4.0 m (or something). Check that it doesn't hog the performance too much.

[nilsbore]

This would be in scitos_costmap_params_3d/costmap_common_params.yaml

[bfalacerda]

This could be the problem, as ours is as tilted down as possible and I didn't see clearing costmap clearing problems here yet. Didn't test it that thoroughly though

[RaresAmbrus]

Can you please also take a look at the framerates of the two cameras ? Use

rostopic hz /head_xtion/depth/image_raw
rostopic hz /chest_xtion/depth/image_raw

This will give us an idea whether the feeds are coming at a proper rate. For us the feeds were about 30 fps after starting scitos_bringup, but when we started the rest of the processes the framerate of the head camera dropped to about 5fps and only came erratically (we haven't tried docking with it like that); after killing the rest of the processes the feeds would get back to 30 fps.

[bfalacerda]

We are steady at 30hz for head and 15 hz for chest with or without other processes running

[nilsbore]

So, @ToMadoRe , are you up for some skyping tomorrow? Also, if you have the opportunity, please take a screenshot of the situation where it fails and check the frequency of the published topics. The drop in framerate can come sporadically so if you can check it when it happens that would be great.

[ToMadoRe]

Sure, i will try to reproduce it and contact you then.

[ToMadoRe]

I think I know one reason for our problem. Looking at the picture below, one can see that the chest camera doesn't see the obstacle anymore and so it probably doesn't update the according costmap. A reason can be that the robot can't stop immediately, but moves on a bit until it eventually stops. Thus, if a person jumps quickly in front of the robot, this problem can happen. It could be improved if the chest cam mount is tilted a bit more. But even then, this problem could occur I think. Also, if we mount the cam steeper, we risk that the robot gets too close to a (high) table or a stairway in our case (also shown in the picture) and can't find a path any more.

[nilsbore]

I think you should tilt it a bit more, that's at least the better option in this situation. Where are the laser scan obstacles in the map? It seems strange that just those four parts are left. It would be cool if you could try this out to see if the map will be cleare out completely when it fails like this:

[nilsbore]

I had a brief look at this and it seems like the costmap is cleared outside or within(not entirely clear) a circle with default radius 3.0 m. This radius can be changed with the conservative_reset_dist parameter of move_base. It would be cool to see what happen if we set this parameter to 0.0, hopefully the whole map will clear.

[nilsbore]

So in launch/move_base_3d.launch put:

`

    <remap from="/move_base/local_costmap/voxel_grid" to="/voxel_grid"/>
    <param name="conservative_reset_dist" value="0.0" />`

The last line is the proposed change.

[ToMadoRe]

Haven't had any problems so far. Your suggestion seems to work. Thanks :)

[nilsbore]

Cool, please let me know as soon as it fails again.

[nilsbore]

If this solves the problems I think we should merge it before the marathon.

[nilsbore]

Again, more of a general discussion thread that is out-of-date. @marc-hanheide Close?