search

OctoMap / octomap

An Efficient Probabilistic 3D Mapping Framework Based on Octrees. Contains the main OctoMap library, the viewer octovis, and dynamicEDT3D.
http://octomap.github.io
1.89k stars 652 forks source link

Obstacle Avoidance? #368

Open HangX-Ma opened 2 years ago

HangX-Ma commented 2 years ago

Hi, I want to realize the obstacle avoidance function for robot path planning. In the following codes I set a threshold for occupancy state . If getOccupancy() return a value larger than 0.5, this node will be regard as an obstacle.

bool GraphSearch::isOccupied(octomap::OcTreeNode* currNode, octomap::point3d &point) {
  return (currNode!=NULL && currNode->getOccupancy() > 0.5);
}

I also use treeNode->setLogOdds() function to update the node occupancy value. If the manipulator can not reach this node or it collides the obstacle at this node. [Note]: I use a planning algorithm similar to A-star, based on Node. 

  auto reNode = tree->search(key);
  reNode->setLogOdds(tree->getClampingThresMaxLog());
  tree->updateInnerOccupancy();

However, the Rviz shows a wrong phenomenon. The first picture shows my initial planned path cross through the obstacle. So I check the all voxel and find where the planned path cross through is empty. Is that normal? If I use the Moveit embeded planner, it can plan a safe path for the manipulator. Can you help me to figure out why this happens? @ahornung occupied voxel all voxel

HangX-Ma commented 2 years ago

Alternatively, I had tried dynamicEDT3D to build the tree by hand. And I use the setNodeValue to update the obstacle information. It seemed to be correct somehow. I need to check further because some errors happened when using my planner .