Ubuntu+ROS: How to get the global map with irregular objects? Why the lidar scans only a simple stick?

[Leon-Redfield]

Ubuntu+ROS: How to get the global map with irregular objects? Why the lidar scans trees only a simple stick?

Hello guys! I'm using Airsim and ROS to apply my navigation algorithms recently. I have two problems.

1. How to get the global map with irregular objects?
   I am trying to get the pointcloud of the global map, and I have seen the solution in Voxel grid construction #3209, but I cannot figure out how to get the PointCloud2 as a message to publish instead of saving it in a file. To be specifically, I don't know how to use this function in C++. So I read the code in function "simcreateVoxelGrid" and I found the core part of it is three for loops, so I wrote a code similar to that as below.
   
   I wrote this code in Airsim_ROS_Wrapper to connect airsim to ROS. Fortunately, it works well. But it has an obvious problem is that these for loops are only useful for very regular objects such as the cubes. So I wonder how to get the pointcloud by adopting the solution in Voxel grid construction #3209 by C++ and ROS as a format of PointCloud2 message to publish instead of saving it in a file? And I wonder if this solution is suitable for very irregular objects such as trees, houses and other complicated objects in a more complex environment?

2. Why the lidar scans only a simple stick?
   I bought an Unreal Environment "Modular Neighborhood Pack" to test and I added some cubes by myself. The sensor I am using is Lidar. When scanning the cubes, it works so well and I used it for mapping. But when I placed a tree, the lidar scans only a simple stick. The apperance is shown as below.
   
   
   As the pictures shown above, the lidar scans work so well when scanning the cubes, but the lidar scans only a simple stick when scanning a tree. I don't know if this problem is from the lidar or the model in Unreal environment. For UAV navigation, I hope that the lidar can scan out the complete tree instead of a simple stick because I don't expect an UAV fly through the tree leaves while navigating.

So all above, I am trying to achieve two things: get the global map in the format of pointcloud message to pulish and get the full scan of the irregular objects in the unreal environment.

I am using the newest version of the Airsim and Ubuntu 20.04. I only use C++ code because I am using ROS to navigate. The settings.json is shown as below. By the way, the reason why I don't use the depth camera is that I want to acquire the 360 degrees scan at one time.(And the processing of depth image is a little bit complex for me)

At last, I have a suggestion. As a Robotic researcher, ROS is the most widespread tool for us to use. The reason why I chose Airsim as my simulation environment is that the unreal environment is very realistic and the APIs of AIrsim are very grounded for most of the time, so I believe more and more robotic researchers will apply their robots in Airsim. Therefore, I really hope to solve these two problems and I am sincerely looking forward to your reply and solutions. Thank you!

[rajat2004]

Unreal Engine uses 2 collision standards - simple and complex. See https://docs.unrealengine.com/en-US/InteractiveExperiences/Physics/SimpleVsComplex/index.html. Check out this comment - #2948 (comment)

Modifying the line mentioned in the comment should handle the Lidar case. If you're using the Binvox API as well, you'll need to modify this line as well -

AirSim/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp

Line 176 in 633e07c

params.bTraceComplex = false;

Wonder if there should be option as well in the API itself regarding this, it can a pretty important factor. Probably best to add this info to the Wiki as well

[Leon-Redfield]

Unreal Engine uses 2 collision standards - simple and complex. See https://docs.unrealengine.com/en-US/InteractiveExperiences/Physics/SimpleVsComplex/index.html. Check out this comment - #2948 (comment)

Modifying the line mentioned in the comment should handle the Lidar case. If you're using the Binvox API as well, you'll need to modify this line as well -

AirSim/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp

Line 176 in 633e07c

params.bTraceComplex = false;

Wonder if there should be option as well in the API itself regarding this, it can a pretty important factor. Probably best to add this info to the Wiki as well

Thank you so much for your solution. At first I tried to change the complexity in unreal engine but there was nowhere to change complexity in my editor. So I tried to use the other method in the comment of #2948. By changing the code in "AirBlueprintLib.cpp" and added "trace_params.bTraceComplex = true;" Finally this works, thank you very much for helping me solve the second problem!!!!!!!!Still looking forward to a solution to my first problem!

[rajat2004]

Does changing the Voxel grid code to use complex collision help in getting data about non-regular objects?

The Voxel grid API is a very expensive one, and IMO not exactly suited to repeated calls. I don't think getting the entire map of the environment is done frequently, or if it's even possible in normal robotics, the map has to be built from the Lidar or Depth data, etc. The saved binvox file can be converted into an octomap or other appropriate format and loaded as the ground truth by your own ROS package.

But if you really do want the simCreateVoxelGrid API to be used as a ROS topic source (probably with a small grid size), the saving to file part can be removed and instead it would return the boolean vector, this should be fairly mechanical changes by changing the return value and the RPC wrapper (see #3611 for more details). Conversion from voxel grid to PointCloud2 or other format will be needed on the ROS wrapper side, which I'm not sure about.

[Leon-Redfield]

Does changing the Voxel grid code to use complex collision help in getting data about non-regular objects?

The Voxel grid API is a very expensive one, and IMO not exactly suited to repeated calls. I don't think getting the entire map of the environment is done frequently, or if it's even possible in normal robotics, the map has to be built from the Lidar or Depth data, etc. The saved binvox file can be converted into an octomap or other appropriate format and loaded as the ground truth by your own ROS package.

But if you really do want the simCreateVoxelGrid API to be used as a ROS topic source (probably with a small grid size), the saving to file part can be removed and instead it would return the boolean vector, this should be fairly mechanical changes by changing the return value and the RPC wrapper (see #3611 for more details). Conversion from voxel grid to PointCloud2 or other format will be needed on the ROS wrapper side, which I'm not sure about.

I'm sorry that I haven't tried to change the Voxel grid code to get the global map. I'm new to this area so that I don't know much about how to get the global map in the format of pointcloud in C++ and ROS. To be honest, I don't need to publish the global map all the time, on the contrary, publishing this message only once is pretty enough for me. So it is okay for me to lower the resolution(I plan to set the resolution 0.1m or 0.05m). Therefore, what I want is as the first problem shows: get the pointcloud of the global map with irregular objects, it would be great if there is any detailed code shown like above, thank you for your reply!

[rajat2004]

So a possible way could be to use the Voxel Grid API to save the binvox file, either before running your ROS package, or as the first thing to be done. After that, the binvox can be converted to octomap (or some other format also) and loaded into the ROS package.

[Leon-Redfield]

So a possible way could be to use the Voxel Grid API to save the binvox file, either before running your ROS package, or as the first thing to be done. After that, the binvox can be converted to octomap (or some other format also) and loaded into the ROS package.

Hi Rajat! I tried to use the api simCreateVoxelGrid and I saved the data to a binvox file. It turns out that it doesn't work well for very detailed and irregular objects despite the fact I set params.bTraceComplex = true;
Take a tree as an example, what I want is that the voxel grid could be detailed enough that only the grids of the tree leaves and branches are occupied, instead of the fact that all scale of a tree is occupied by the voxel grid. So this really upsets me and I feel depressed by the fact that this api could only be applied to get the voxel grid of very regular objects.
Still very thankful for helping me to solve the second problem and give me some advice on my first problem. I wonder if there's any other api which could satisfies my requirement or is it possible to update this api to make it more detailed in the near future?

[zimmy87]

Hey @Leon-Redfield! What do you mean by "all scale of a tree is occupied by the voxel grid", is setting params.bTraceComplex = true; not resulting in an accurate representation of the tree?

As for your first question, simCreateVoxelGrid is designed to fill a vector of bools, whereas it looks like the pointcloud structure you're trying to fill is a vector of XYZ points. To adapt your algorithm for irregular shapes, you can call OverlapBlockingTestByChannel inside of your nested for loops, but instead of storing the result like in simCreateVoxelGrid, use it as a conditional for whether or not to store the input position parameter in your pointcloud. Something like the following might work:

float scale_cm = resolution_ * 100;
FCollisionQueryParams params;
params.bFindInitialOverlaps = true;
params.bTraceComplex = false;
params.TraceTag = "";
for (lx = -cube_scale.x()/2; lx<cube_scale.x()/2+resolution_; lx+=resolution_)
{
    for (ly = -cube_scale.y()/2; ly<cube_scale.y()/2+resolution_; ly+=resolution_)
    {
        for (lz = -cube_scale.z()/2; lz<cube_scale.z()/2+resolution_; lz+=resolution_)
        {
            Eigen::Vector3d obs_body;
            obs_body << lx,ly,lz;
            Eigen::Vector3d obs_world;
            obs_world = body2world*obs_body+position;
            pcl::PointXYZ pt;
            pt.x = obs_world[0];
            pt.y = obs_world[1];
            pt.z = obs_world[2];
            if (simmode_->GetWorld()->OverlapBlockingTestByChannel(FVector(pt.x*scale_cm,pt.y*scale_cm,pt.z*scale_cm), FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params))
            {
                cloudMap.points.push_back(pt);
            }
        }
    }
}

[Leon-Redfield]

Hey @Leon-Redfield! What do you mean by "all scale of a tree is occupied by the voxel grid", is setting params.bTraceComplex = true; not resulting in an accurate representation of the tree?

As for your first question, simCreateVoxelGrid is designed to fill a vector of bools, whereas it looks like the pointcloud structure you're trying to fill is a vector of XYZ points. To adapt your algorithm for irregular shapes, you can call OverlapBlockingTestByChannel inside of your nested for loops, but instead of storing the result like in simCreateVoxelGrid, use it as a conditional for whether or not to store the input position parameter in your pointcloud. Something like the following might work:

float scale_cm = resolution_ * 100;
FCollisionQueryParams params;
params.bFindInitialOverlaps = true;
params.bTraceComplex = false;
params.TraceTag = "";
for (lx = -cube_scale.x()/2; lx<cube_scale.x()/2+resolution_; lx+=resolution_)
{
    for (ly = -cube_scale.y()/2; ly<cube_scale.y()/2+resolution_; ly+=resolution_)
    {
        for (lz = -cube_scale.z()/2; lz<cube_scale.z()/2+resolution_; lz+=resolution_)
        {
            Eigen::Vector3d obs_body;
            obs_body << lx,ly,lz;
            Eigen::Vector3d obs_world;
            obs_world = body2world*obs_body+position;
            pcl::PointXYZ pt;
            pt.x = obs_world[0];
            pt.y = obs_world[1];
            pt.z = obs_world[2];
            if (simmode_->GetWorld()->OverlapBlockingTestByChannel(FVector(pt.x*scale_cm,pt.y*scale_cm,pt.z*scale_cm), FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params))
            {
                cloudMap.points.push_back(pt);
            }
        }
    }
}

Hi @zimmy87! Thank you for your reply and suggestion! Right now what I have done is that I successfully used the functionsimCreateVoxelGrid and store the voxel grid into a binvox file. After that I visualize the voxel grid by viewvox. I found it seems that all the scale of a tree is occupied by the voxel grid. For example, in real world, the part between two branches of a tree is vacancy(suppose a tree is "Y" shape), but it is occupied in the voxel grid of what I got. I'm sorry but I haven't transform the voxel grid into pointcloud, so I could't visualize it in Rviz right now. I'm trying to get the pointcloud and publish it as the first problem I asked. Have you ever tried to get the pointcloud of the global map and is it possible to be detailed enough? And did you mean by changing the code of the function simCreateVoxelGrid?

[zimmy87]

When you say "all the scale of a tree is occupied by the voxel grid", are you referring to cells containing leaves being occupied? If so, it's important to note that all the trees in the AirSimNH environment (I suspect the trees in the "Modular Neighborhood Pack" are set up very similarly) have a complex collision that includes all the leaves. Since you're setting params.bTraceComplex = true, you will see a dense volume wherever there's leaves to collide against. If you wanted to only test where there are branches in the tree, you will probably have to create new collision primitives.