converting 3D octomap to 3D occupancy grid

[ghost]

hi, I would like use 3D occupancy grid for path planning. Now i have message topic /rtabmap/octomap_full /rtabmap/octomap_binary and i want to convert that into 3D occupancy grid message. The message /rtabmap/octomap_grid has the information of width and height not the length. For 3D path planning, i need length, width and height. How to convert the octomap_msgs to 3D occupancy grid message (not 2d)? any idea?

many thanks in advance,

[matlabbe]

I am not aware of a standard 3D occupancy grid msg other than octomap_msgs/Octomap (which is /rtabmap/octomap_full and /rtabmap/octomap_binary), can you link one? What kind of message does your 3D path planning node require? In the octomap msg, you have the resolution of the cells.

cheers, Mathieu

[ghost]

My path planning node requires grid length, grid width, grid height, x_min, y_min, z_min (origin) of the cell . In theoctomap_msgs/Octomap msg i have the below info

std_msgs/Header header
bool binary
string id
float64 resolution
int8[] data

Inside this data, is it possible to access the grid length, grid width, grid height, x_min, y_min, z_min (origin) of the cell? or how to convert the octomap_msg to get the above information?

[matlabbe]

Use octomap_msgs conversion methods: http://docs.ros.org/kinetic/api/octomap_msgs/html/namespaceoctomap__msgs.html to deserialize the octomap topic. You will get an octomap::AbstractOcTree object, which would have all information you need (an octomap::AbstractOcTree* that you would cast into a octomap::ColorOcTree* as rtabmap creates only ColorOcTree type).

cheers, Mathieu

[ghost]

many thanks for the info. I tried like this,

octomap_msgs::Octomap octomap_data(*msg);
octomap::AbstractOcTree* my_tree = octomap_msgs::binaryMsgToMap(octomap_data);

but i getting the error while compiling error: ‘binaryMsgToMap’ is not a member of ‘octomap_msgs’ what is missing here?

[matlabbe]

What is your CMakeLists.txt file? Make sure you have octomap_msgs package added to dependencies and that you have the conversion.h file installed (which ros distro are you on?):

#include <octomap_msgs/conversions.h>

cheers, Mathieu

[ghost]

many thanks! I'm using ros-indigo with ubuntu 14.04 if i add header file , it is producing different error like below, (assume because of the octomap binaries)

Linking CXX executable /home/arun/Workspace/catkin/devel/lib/rtabmap_ros/rtabmapviz
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::ColorOcTreeNode::expandNode()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomath::Pose6D::inv() const'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::ColorOcTreeNode::pruneNode()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::OcTreeNode::OcTreeNode()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomath::Pose6D::transform(octomath::Vector3 const&) const'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::OcTreeNode::addValue(float const&)'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::OcTreeNode::OcTreeNode()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::OcTreeNode::getMaxChildLogOdds() const'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::OcTreeNode::~OcTreeNode()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::Pointcloud::~Pointcloud()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::ColorOcTreeNode::writeValue(std::ostream&) const'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::OcTreeNode::~OcTreeNode()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::AbstractOccupancyOcTree::AbstractOccupancyOcTree()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::OcTreeNode::createChild(unsigned int)'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::Pointcloud::transform(octomath::Pose6D)'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomath::Quaternion::Quaternion(octomath::Quaternion const&)'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomath::operator<<(std::ostream&, octomath::Vector3 const&)'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::Pointcloud::Pointcloud()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomath::Pose6D::~Pose6D()'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::Pointcloud::Pointcloud(octomap::Pointcloud const&)'
/home/arun/Workspace/catkin/devel/lib/libprp.so: undefined reference to `octomap::ColorOcTreeNode::readValue(std::istream&)'
collect2: error: ld returned 1 exit status
make[2]: *** [/home/arun/Workspace/catkin/devel/lib/prp/prp_node] Error 1
make[1]: *** [prp/CMakeFiles/prp_node.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....

since i have octomap binaries (apt-get install ros-indigo-octomap) and octomap packages in the ros workspace, it is not finding the right one.

How to properly link the octomap libraries without affecting rtabmap_ros packages?

[ghost]

Now i solved the issue using the octomap_ros My cmake list

find_package(catkin REQUIRED COMPONENTS
  nav_core
  roscpp
  rospy
  std_msgs
  tf
  octomap_ros
  octomap_msgs
)

and the header files are

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap/octomap.h>

many thanks for the support!