felixm...@gmail.com
unread,Mar 4, 2013, 5:24:12 AM3/4/13Sign in to reply to author
Sign in to forward
You do not have permission to delete messages in this group
Either email addresses are anonymous for this group or you need the view member email addresses permission to view the original message
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