castRay always returns empty

asked 2016-11-04 07:36:32 -0600

kotoko gravatar image

Hi

I'm doing the typical casting a ray to see if it is occupied.
I wasn't understanding the results so I decided to do a reality check, to make sure I was using the function correctly.
What I did was to transverse the tree and use the coordinates of the center of the occupied cells to call the castRay function.

     octomap::point3d min; min.x() = -range; min.y() = -range; min.z() = -1;
     octomap::point3d max; max.x() = range; max.y() = range; max.z() = 4;
     std::cout << "printing occupancy \n";
     for(octomap::OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(min,max), endLoop=octree->end_leafs_bbx(); 
        it!= endLoop; 
        ++it)
    {
        octomap::OcTreeNode * temp = octree->search(it.getKey());
        if (temp != NULL && octree->isNodeOccupied(temp))
        {   
            std::cout << "(" << it.getX() << "; " << it.getY()<< "; " << it.getZ() << ")   ";
            std::cout << it.getCoordinate() << "    ";
            // Testing ray
            octomath::Vector3 end;
            bool occupied = octree->castRay(octomath::Vector3(0.f, 0.f, 0.f),
                it.getCoordinate(),
                end,
                false,
                range+10
                );
            std::cout << " according to ray it is occupied? "<< occupied << "\n";
        }   
    }  
    std::cout << "\n";

I was expecting to always get

(2.55; 0.45; 0.85) (2.55 0.45 0.85) according to ray it is occupied? 1

But instead I got

(2.55; 0.45; 0.85) (2.55 0.45 0.85) according to ray it is occupied? 0

In a lot of different positions.

Why is it considered occupied as a node but on the ray cast it is empty?

edit retag flag offensive close merge delete