SegFault in DynamicEDTOctomap::initializeOcTree()

306 views
Skip to first unread message

felixm...@gmail.com

unread,
Mar 4, 2013, 5:24:12 AM3/4/13
to oct...@googlegroups.com
Hi,

I installed octomap v.1.5.3 to implement a collision checker based on dynamicEDT3D.
Unfortunately, I get SIGSEGV Segmentation fault during the creation of the DynamicEDT3DOctomap distmap (see StackTrace below!)


(gdb) run
Starting program: /home/fxm/projects/miror/miror/miror_arm_navigation/bin/miror_SnakeArmTCPPlanning
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
Reading binary octree type OcTree
read in tree, 2618023 leaves
Compound state [
RealVectorState [11.15 7.1 1.6]
SO3State [0 0 0 1]
]
Compound state [
RealVectorState [9.06 6.5 9.4]
SO3State [0 0 0 1]
]
Start+Goal set
Planner set
Debug:   Planner range detected to be 2.689028
[New Thread 0x7ffff5cda700 (LWP 19238)]
GoalPos: 11.15, 7.1, 1.6
Metric min: -14.558,-18.5031,-0.33339
Metric min: -14.558,-18.5031,-0.33339
Metric max: 18.781,14.7803,8.11249
Metric max: 18.781,14.7803,8.11249

Program received signal SIGSEGV, Segmentation fault.
0x00007ffff5e1342e in DynamicEDTOctomap::initializeOcTree(octomath::Vector3, octomath::Vector3) () from /usr/local/lib/libdynamicedt3d.so
(gdb) bt
#0  0x00007ffff5e1342e in DynamicEDTOctomap::initializeOcTree(octomath::Vector3, octomath::Vector3) () from /usr/local/lib/libdynamicedt3d.so
#1  0x00007ffff5e14a16 in DynamicEDTOctomap::DynamicEDTOctomap(float, octomap::OcTree*, octomath::Vector3, octomath::Vector3, bool) () from /usr/local/lib/libdynamicedt3d.so
#2  0x00007ffff76e7303 in SnakeArmCollisionChecker::doCollisionChecking (this=0x613b30, tcp_config=...)
    at /home/fxm/projects/miror/miror/miror_arm_collision/common/src/snake_arm_collision_checker.cpp:161
#3  0x000000000040937d in MyStateValidityChecker::isValid (this=0x613a50, state=<optimized out>)
    at /home/fxm/projects/miror/miror/miror_arm_navigation/common/include/miror_arm_navigation/miror_arm_collision_checker.h:73
#4  0x00007ffff7a48efe in ompl::base::PlannerInputStates::nextStart() () from /opt/ros/fuerte/lib/libompl.so.5
#5  0x00007ffff7ad0dd3 in ompl::geometric::RRT::solve(ompl::base::PlannerTerminationCondition const&) () from /opt/ros/fuerte/lib/libompl.so.5
#6  0x00007ffff7a48509 in ompl::base::Planner::solve(double) () from /opt/ros/fuerte/lib/libompl.so.5
#7  0x00007ffff7a79b34 in ompl::geometric::SimpleSetup::solve(double) () from /opt/ros/fuerte/lib/libompl.so.5
#8  0x00000000004074f5 in plan (start_config=..., goal_config=...) at /home/fxm/projects/miror/miror/miror_arm_navigation/common/src/miror_SnakeArmTCPPlanning.cpp:205
#9  0x0000000000406864 in main (argc=<optimized out>, argv=<optimized out>) at /home/fxm/projects/miror/miror/miror_arm_navigation/common/src/miror_SnakeArmTCPPlanning.cpp:236





Here is how I initialized the OcTree structure (from a binary tree file)


SnakeArmCollisionChecker::SnakeArmCollisionChecker(char* filename)
{
    tree = new octomap::OcTree(0.01);

    //read in octotree
    tree->readBinary(filename);

    std::cout<<"read in tree, "<<tree->getNumLeafNodes()<<" leaves "<<std::endl;
}




Finally, here is my funcion to check for collision (i.e. compute distance to closest obstacle):


bool SnakeArmCollisionChecker::doCollisionChecking(std::vector<double> &tcp_config)
{
    double clearance = 0.05;    //5 cm
   
    octomap::point3d p(tcp_config[0],tcp_config[1],tcp_config[2]);
    std::cout<<"GoalPos: "<<p.x()<<", "<<p.y()<<", "<<p.z()<<std::endl;
           
    ///find good region-of-interest
    //ToDo: Verify this!!!
    double x,y,z;
    tree->getMetricMin(x,y,z);
    std::cout<<"Metric min: "<<x<<","<<y<<","<<z<<std::endl;
    //octomap::point3d min(std::max(x,p.x()-clearance),std::max(y,p.y()-clearance),std::max(z,p.z()-clearance));
    //octomap::point3d min(x,y,z);
    octomath::Vector3 min(x,y,z);
    std::cout<<"Metric min: "<<min.x()<<","<<min.y()<<","<<min.z()<<std::endl;
    tree->getMetricMax(x,y,z);
    std::cout<<"Metric max: "<<x<<","<<y<<","<<z<<std::endl;
    //octomap::point3d max(std::min(x,p.x()+clearance),std::min(y,p.y()+clearance),std::min(z,p.z()+clearance));
    //octomap::point3d max(x,y,z);
    octomath::Vector3 max(x,y,z);
    std::cout<<"Metric max: "<<max.x()<<","<<max.y()<<","<<max.z()<<std::endl;
   

    bool unknownAsOccupied = true;
    unknownAsOccupied = false;
    float maxDist = clearance;
    //- the first argument ist the max distance at which distance computations are clamped
    //- the second argument is the octomap
    //- arguments 3 and 4 can be used to restrict the distance map to a subarea
    //- argument 5 defines whether unknown space is treated as occupied or free
    //The constructor copies data but does not yet compute the distance map
    DynamicEDTOctomap distmap(maxDist, tree, min, max, unknownAsOccupied);

    //This computes the distance map
    distmap.update();

    //This is how you can query the map
    octomap::point3d closestObst;
    float distance;

    distmap.getDistanceAndClosestObstacle(p, distance, closestObst);


    //std::cout<<"distance at point "<<p.x()<<","<<p.y()<<","<<p.z()<<" is "<<distance<<std::endl;
    if(distance < distmap.getMaxDist())
        //std::cout<<"closest obstacle to "<<p.x()<<","<<p.y()<<","<<p.z()<<" is at "<<closestObst.x()<<","<<closestObst.y()<<","<<closestObst.z()<<std::endl;
    if(distance < clearance)
    {
        //std::cout<<"State in collision"<<std::endl;
        return false;
    }
     

    //std::cout<<"State is ok"<<std::endl;
    return true;
}





Initially, I would like to specify a relevant region-of-interest around the tcp_config position, but I also get the same Segmentation fault, when I use tree->getMetricMin(x,y,z)/tree->getMetricMax(x,y,z) - values.

I can't see, what I am doing wrong. Any help is appreciated. Thanks!


Best regards,
Felix




Christoph Sprunk

unread,
Mar 4, 2013, 6:09:27 AM3/4/13
to oct...@googlegroups.com
Hi Felix,

my first guess would be that you run out of memory. The DynamicEDT3D
will allocate a full 3D voxel grid for the specified area and your area
seems to be pretty large for a resolution of 0.01 m.

To verify this, could you try the following:
- use a much smaller octomap and maybe a coarser resolution
- use the original octomap but change your points "min" and "max" to
describe a much smaller area

Hope this helps and I understood the problem correctly, if not please
write back.

Best,
Christoph
> --
> You received this message because you are subscribed to the Google
> Groups "OctoMap developers and users discussion" group.
> To unsubscribe from this group and stop receiving emails from it, send
> an email to octomap+u...@googlegroups.com.
> To post to this group, send email to oct...@googlegroups.com.
> Visit this group at http://groups.google.com/group/octomap?hl=en.
> For more options, visit https://groups.google.com/groups/opt_out.
>
>

felixm...@gmail.com

unread,
Mar 4, 2013, 10:46:56 AM3/4/13
to oct...@googlegroups.com
Dear Christoph,

thanks for the immediate reply.
Unfortunately using a smaller binary file and reducing the OcTree resolution does not help.
I get the same SegFault also for an OcTree with only 80k leaves and resolution 0.5.
The min and max of getMetricMin/getMetricMax for this binary tree are -1..1,-1..1,-0.1...1.9.
Also, as you can see in the code above, I wanted to restrict the bounding box of the distmap to be of the size 2*clearance around tcp_config, which is much smaller than the actuall OcTree MetricMin/Max.

The strange thing is, that I had it running even with the large binary tree and fine resolution before I upgraded to 12.04 and Fuerte.
Unfortunately, I cannot recall which version of OctoMap, I was using...Where there any changes recently?

Any other suggestions?

One thing, that I find strange is that the function heads of DynamicEDTOctomap::DynamicEDTOctomap and DynamicEDTOctomap::initializeOcTree state that min and max are to be given in octomath::Vector3 where in v1.5.3 the function heads are using octomap::point3d? Any idea what''s wrong there?


Best,
Felix

Christoph Sprunk

unread,
Mar 4, 2013, 11:08:17 AM3/4/13
to oct...@googlegroups.com
Hi Felix,

On 03/04/2013 04:46 PM, felixm...@gmail.com wrote:
> Dear Christoph,
>
> thanks for the immediate reply.
> Unfortunately using a smaller binary file and reducing the OcTree
> resolution does not help.
> I get the same SegFault also for an OcTree with only 80k leaves and
> resolution 0.5.
> The min and max of getMetricMin/getMetricMax for this binary tree are
> -1..1,-1..1,-0.1...1.9.
> Also, as you can see in the code above, I wanted to restrict the
> bounding box of the distmap to be of the size 2*clearance around
> tcp_config, which is much smaller than the actuall OcTree MetricMin/Max.
>
> The strange thing is, that I had it running even with the large binary
> tree and fine resolution before I upgraded to 12.04 and Fuerte.
> Unfortunately, I cannot recall which version of OctoMap, I was
> using...Where there any changes recently?
ok, that's a good information. There were no changes recently so if it
worked with an older version it should still work.

>
> Any other suggestions?
I would suggest trying two things:
- Did you make sure that you recompiled dynamicEDT3D after upgrading
octomap? Maybe there is some linking against old versions going on.
- Try to load the octomap with the exampleEDTOctomap.cpp from
src/examples and see if the error also occurs there. This way we could
get a minimum (not) working example going.

>
> One thing, that I find strange is that the function heads of
> DynamicEDTOctomap::DynamicEDTOctomap and
> DynamicEDTOctomap::initializeOcTree state that min and max are to be
> given in octomath::Vector3 where in v1.5.3 the function heads are using
> octomap::point3d? Any idea what''s wrong there?
The reason for this is that point3d is a typedef of octomath::Vector3.

Best,
Christoph
> > an email to octomap+u...@googlegroups.com <javascript:>.
> > To post to this group, send email to oct...@googlegroups.com
> <javascript:>.
> <http://groups.google.com/group/octomap?hl=en>.
> > For more options, visit https://groups.google.com/groups/opt_out
> <https://groups.google.com/groups/opt_out>.

felixm...@gmail.com

unread,
Mar 4, 2013, 11:32:41 AM3/4/13
to oct...@googlegroups.com
Hi,

indeed, there was an old lib for dynamicEDT3D in /usr/local/...*umpf*...sorry
After removing, I tried to re-install octomap v1.5.3.
But now, I get the following error during compilation:

Scanning dependencies of target dynamicedt3d
[ 90%] Building CXX object dynamicEDT3D/src/CMakeFiles/dynamicedt3d.dir/dynamicEDT3D.cpp.o
[ 92%] Building CXX object dynamicEDT3D/src/CMakeFiles/dynamicedt3d.dir/dynamicEDTOctomap.cpp.o
/home/fxm/projects/miror/octomap/dynamicEDT3D/src/dynamicEDTOctomap.cpp: In member function ‘void DynamicEDTOctomap::initializeOcTree(octomap::point3d, octomap::point3d)’:
/home/fxm/projects/miror/octomap/dynamicEDT3D/src/dynamicEDTOctomap.cpp:86:33: error: ‘class octomap::OcTree’ has no member named ‘coordToKey’
/home/fxm/projects/miror/octomap/dynamicEDT3D/src/dynamicEDTOctomap.cpp:87:33: error: ‘class octomap::OcTree’ has no member named ‘coordToKey’
/home/fxm/projects/miror/octomap/dynamicEDT3D/src/dynamicEDTOctomap.cpp: In member function ‘void DynamicEDTOctomap::worldToMap(const point3d&, int&, int&, int&) const’:
/home/fxm/projects/miror/octomap/dynamicEDT3D/src/dynamicEDTOctomap.cpp:189:35: error: ‘class octomap::OcTree’ has no member named ‘coordToKey’
/home/fxm/projects/miror/octomap/dynamicEDT3D/src/dynamicEDTOctomap.cpp: In member function ‘void DynamicEDTOctomap::mapToWorld(int, int, int, octomap::point3d&) const’:
/home/fxm/projects/miror/octomap/dynamicEDT3D/src/dynamicEDTOctomap.cpp:196:14: error: ‘class octomap::OcTree’ has no member named ‘keyToCoord’
make[2]: *** [dynamicEDT3D/src/CMakeFiles/dynamicedt3d.dir/dynamicEDTOctomap.cpp.o] Error 1
make[1]: *** [dynamicEDT3D/src/CMakeFiles/dynamicedt3d.dir/all] Error 2
make: *** [all] Error 2
fxm@sony-fxm:~/projects/miror/octomap/build$




By the way, octovis doesn't build as well, but I commented it out in the CMakeLists.


V1.5.4 doesn't work as well...
Is this a known issue? Is there a fix for that?




Am Montag, 4. März 2013 17:08:17 UTC+1 schrieb sprunkc:
Hi Felix,

felixm...@gmail.com

unread,
Mar 4, 2013, 11:41:12 AM3/4/13
to oct...@googlegroups.com
By the way, the error log for octovis:

Scanning dependencies of target octovis
[ 71%] Building CXX object octovis/CMakeFiles/octovis.dir/src/main.cpp.o
[ 73%] Building CXX object octovis/CMakeFiles/octovis.dir/src/ViewerGui.cpp.o
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp: In member function ‘void octomap::ViewerGui::on_actionSave_file_triggered()’:
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:813:9: error: ‘AbstractOccupancyOcTree’ was not declared in this scope
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:813:34: error: ‘ot’ was not declared in this scope
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:813:52: error: expected type-specifier before ‘AbstractOccupancyOcTree’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:813:52: error: expected ‘>’ before ‘AbstractOccupancyOcTree’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:813:52: error: expected ‘(’ before ‘AbstractOccupancyOcTree’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:813:76: error: expected primary-expression before ‘>’ token
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:813:81: error: expected ‘)’ before ‘;’ token
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp: In member function ‘void octomap::ViewerGui::on_actionDelete_nodes_in_selection_triggered()’:
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:879:19: error: ‘class octomap::OcTree’ has no member named ‘deleteNode’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp: In member function ‘void octomap::ViewerGui::on_actionDelete_nodes_outside_of_selection_triggered()’:
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:902:22: error: ‘class octomap::OcTree’ has no member named ‘coordToKeyChecked’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:902:65: error: ‘class octomap::OcTree’ has no member named ‘coordToKeyChecked’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:913:21: error: ‘class octomap::OcTree’ has no member named ‘deleteNode’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp: In member function ‘void octomap::ViewerGui::setNodesInBBX(const point3d&, const point3d&, float)’:
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:976:17: error: ‘class octomap::OcTree’ has no member named ‘coordToKeyChecked’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:977:17: error: ‘class octomap::OcTree’ has no member named ‘coordToKeyChecked’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp: In member function ‘void octomap::ViewerGui::setNodesInBBX(const point3d&, const point3d&, bool)’:
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:1004:17: error: ‘class octomap::OcTree’ has no member named ‘coordToKeyChecked’
/home/fxm/projects/miror/octomap/octovis/src/ViewerGui.cpp:1005:17: error: ‘class octomap::OcTree’ has no member named ‘coordToKeyChecked’
make[2]: *** [octovis/CMakeFiles/octovis.dir/src/ViewerGui.cpp.o] Error 1
make[1]: *** [octovis/CMakeFiles/octovis.dir/all] Error 2

Armin Hornung

unread,
Mar 4, 2013, 11:50:30 AM3/4/13
to oct...@googlegroups.com
Hi Felix,

from all these errors it seems that your are mixing different versions
of OctoMap. Do you have an older version globally installed, or in your
ROS workspace? octovis and libdynamicEDT3D seem to pick one up that does
not provide the functions introduced in 1.5. So you are compiling
octovis 1.5 and dynamicEDT 1.5 against octomap < 1.5.

You can actually install octomap and octovis 1.5 in ROS groovy as binary
packages: ros-groovy-octomap ros-groovy-octovis. Fuerte only provies
version 1.4.

After sourcing the ROS setup.bash compile *just* dynamicEDT3D version
1.5, not the complete octomap distribution. Or compile the complete
octomap distribution (as downloaded source archive) but ensure that you
are not in a ROS workspace that has an older version installed (or any
other version of octomap still in /usr/local).

It's also possible to overlay your local ROS version with a development
checkout (source version) of OctoMap, but that's a bit more complicated:
http://answers.ros.org/question/52844/overlay-octomap-with-devel-branch-source-for-ros/?answer=52895#post-id-52895


Best,
Armin

--
Armin Hornung
Humanoid Robots Lab, Albert-Ludwigs-Universit�t Freiburg
Contact: http://www.informatik.uni-freiburg.de/~hornunga

felixm...@gmail.com

unread,
Mar 5, 2013, 6:50:55 AM3/5/13
to oct...@googlegroups.com
Dear Armin,

indeed there were some left-over octomap stuff around.
After removing it, I was able to compile v1.5.3 from source and use it in my application.
I still have to be careful with the large OcTree structure, resolution and region-of-interest, though.

Unfortunately, I am not able to fully go for ROS Groovy yet already due to a lot of dependencies and lack of support within a bundge of other projects.
I'll definitely try to go for it, as soon as possible.

Thanks for your help, guys!


Best,
Felix
Reply all
Reply to author
Forward
0 new messages