Question regarding Octomap & Planner

[SwiftGust]

Hello,
I have set up cluttered environment and playing with this repository.
What I have set-up here is world with walls, bars and trees are up ahead but found not satisfactory result.
If planner works properly, drone can fly almost straight ahead with some small course correction

I have managed StereoSGBM parameter little bit tuned.

PreFilterCap: 32
sgbmWinSize: 7
MinDisparity: 0
UniquenessRatio: 65
SpeckleWindowSize: 500
SpeckleRange: 1
Disp12MaxDiff: 1
TextureThreshold: 0
Width: 640
Height: 480
Scale: 1

In disparity map, I can clearly see obstacles with some noise, but Octomap result is quite different.
It is mapped as some radial way regarding walls in left & right front.

map.bt.tar.gz

And also I'm wondering that does InformedRRT* check how much space vehicle does occupy.
It plans like it tries to move through the wall if it finds small space in thick wall
and does not avoid bars so far.
However, it does pause in front of bars for few seconds then goes straight head into.

I'd like to know more about these issues so maybe I can help to improve.
Thank you.

[ayushgaud]

And also I'm wondering that does InformedRRT* check how much space vehicle does occupy.
It plans like it tries to move through the wall if it finds small space in thick wall
and does not avoid bars so far.

Here you will see that I have assumed the drone to be a cube of side 1.5m so that should't happen
https://github.com/ArduPilot/OctomapPlanner/blob/master/src/Planner.cpp#L27

Could you also share the visualization of the map over Gazebo? I think the issue might be with the odometry which I am using for the pose, because it's only coming at 5Hz and sometimes messages tend to buffer a lot and causes delay which leads to poor synchronization between the view and the actual pose and leads to poor Octomap construction. Maybe try reducing the velocity or increasing the odometry publishing rate.
Please let me know if you still face issues, I'll also take a look into it.
Thanks for reaching out, I helps a lot when you guys give feedbacks and which also helps in improving the quality of code and find bugs.

[SwiftGust]

Hello,

I have disabled visualization feature since it causes severe lag when planning completed,
And I thought off-points problem was from actual obstacle are from visualization problem
But from your explanation cause of the problem was the delayed communication.

I did run simulation again with visualization up,
couldn't find where vehicle data comes from so reduced interval from 10 to 1 (ms if explanation is correct) and built node again.

https://github.com/ArduPilot/OctomapPlanner/blob/master/src/mavlink_comm.cpp#L26.

This works, planning result is quite better than before
I'm still getting confused that most of problem happens when it is in initial scanning
while hovering within few centi-meter level radius

I think time-based replanning is somewhat unsatisfactory so need some way around.
It would be great if it could perfectly plan every interval
but, it often come up with worse solution than before in my virtual world.

Octomap Visualized in Gazebo here :

I'm learning a lot from this repository and it's very interesting too.

I'm still fully not understand this repository, but I'm hoping to improve this.

Thank you.

[SwiftGust]

I think we need interactive ways to set-up parameters, set up the goal
and not to depend the start position.
And also should be able to plan real-time base on current position and Octomap updates.

I tried Avoidance repository of PX4
Code : https://github.com/px4/avoidance
Video : https://www.youtube.com/watch?v=bdlvSU93gbI

Global planner of PX4 works great, this one looks like called risk based planning.
It plans utilizing all 3D spaces as obstacle pop with planning real-time based on depth camera(like real sense)
The code depends heavily on ROS.

[ayushgaud]

I recently came across that code myself. It's an interesting implementation. Since the OctomapPlanner was initially designed in such a manner that user does not require any experience with ROS hence a lot of boilerplate code was needed to be written. That's the main reason I wasn't able to work on the interface part of it.

I'm still getting confused that most of problem happens when it is in initial scanning
while hovering within few centi-meter level radius

Currently, the major issues I suspect is with the drone odometry since we are not using ground truth Gazebo pose information and relying on the SITL EKF for it. If we can somehow improve the pose estimates, it would definitely improve the results.

And also should be able to plan real-time base on current position and Octomap updates.

Usually, planning is done in two steps, global and local. In our case, we only have a global planner which takes a while to compute the plan and hence it is not as reactive. Although, in most cases where the environment is static and the drone is slow, this would do the job. I have added the code for realsense gazebo model and will soon try to integrate it with the planner. Maybe we might see some improvements with it.

[SwiftGust]

I don't think delayed odometry is not major factor alone here to cause such a problem while it is staying in a few cm radius and I think we should not use gazebo real data for less dependency, so that simply used in actual hardware.

Initial mapping result in comments above shows mapped obstacles are at quite off points from the actual position (Walls are few meters far from actual position )

What I'm suspect is that something in the camera parameter - stereoSGBM - Octomap has some problem with respect to mapping exact position, maybe real sense can help about.

I know the concept of global planning & local planning but, PX4 Avoidance has managed to global planner to compute path in almost real-time with risk-based planning although no actual reference is given unlike local planner (3DVFH+)

This risk based planning with RRT algorithm looks like
can reduce search path extremely so that can be computed real-time
in this paper
Risk based motion planning and navigation in uncertain dynamic environment
( https://hal.inria.fr/inria-00526601/document )

I think this would be interesting ways to start improve OctomapPlanner too.

[SwiftGust]

For the note, I have added iris model with Intel Realsense, use this one if this is needed.
https://github.com/SwiftGust/ardupilot_gazebo/tree/dev
This also require Intel Realsense for Gazebo https://github.com/intel/gazebo-realsense

You can find depthmap from realsense topic under the name ~/iris/rs/stream/depth

[BreederBai]

@SwiftGust @ayushgaud Hi, I am interested in motion planning. I recently reproduced HKUST-Aerial-Robotics/Fast-Planner, I've run PX4/PX4-Avoidance before, and I've been following ArduPilot/OctomapPlanner for a long time. It has been more than 3 years since this project was created, and now there are more and more open source solutions for motion planning. The method used seems to be similar to that of this project. I wonder if there is a big difference between RRT*, A*, VFH+ used by ArduPilot/OctomapPlanner, HKUST-Aerial-Robotics/Fast-Planner, PX4/PX4-Avoidance? If I were to create a motion planning program, how would I choose these algorithms?