Obstacle Avoidance?

[HangX-Ma]

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

[HangX-Ma]

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 .