how to create octomap_masgs for octomap_rviz_plugins
I want to use octmap_rviz_plugins to display the map in rviz ( I DONT want to use octomap_server) 1- OccupancyGrid: Displays 3D occupancy grids generated from compressed octomap messages. 2- OccupancyMap : Displays projected 2D occupancy maps generated from compressed octomap messages.
I should construct a msg of type octmap_msgs::Octmap as I understood..
How can I do that from the following code and publish this msg so I can display it in rviz :
void laserCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
if(!listener_.waitForTransform(scan_in->header.frame_id,
"base_link",
scan_in->header.stamp +ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment),
ros::Duration(1.0))){
return;
}
sensor_msgs::PointCloud msg;
projector_.transformLaserScanToPointCloud("base_link",*scan_in, msg,listener_);
OcTree st_tree (0.01); // create empty tree with resolution 0.1
octomap::Pointcloud st_cld;
for(int i = 0;i<msg.points.size();i++){
point3d endpoint((float) msg.points[i].x,(float) msg.points[i].y,(float) msg.points[i].z);
st_cld.push_back(endpoint);
}
point3d origin(0.0,0.0,0.0);
st_tree.insertPointCloud(st_cld,origin);
st_tree.updateInnerOccupancy();
// the folliwing line will generate for me the map after I close the program BUT I want to display it in Rviz online
st_tree.writeBinary("static_occ.bt");
// HERE
// how can I create the msg or the published topic that I will use it in rviz plugin
// ???????????????????????????????????????????????????
octomap_msgs::Octomap fullMap ;
how can I creat this msg and publish it ???? and is it the right format for the mentioned plugins above ???
Edit::
I completed the code as the follwoing:
octomap_msgs::Octomap octomap ;
octomap.binary = 1 ;
octomap.id = 1 ;
octomap.resolution =0.05 ;
bool res = octomap_msgs::fullMapToMsg(*st_tree, octomap);
//octomap.data = td_vector_to_py_list(octomap.data) ;
octmap_pub.publish(octomap) ;
I could only see the OccupancyMap after I did a ststic tansformation betweent he odom frame and map frame.. But I still have a warning "No map received"
I couldn't visualize the OccupancyGrid and I got the error in RVIZ saying: "Failed to transform from frame [] to frame [odom] "
EDIT2:
I changed the code to the follwoing:
point3d origin(0.0,0.0,0.0);
st_tree->insertPointCloud(st_cld,origin);
st_tree->updateInnerOccupancy();
st_tree->writeBinary("static_occ.bt");
octomap_msgs::Octomap octomap ;
octomap.binary = 1 ;
octomap.id = 1 ;
octomap.resolution =0.05 ;
octomap.header.frame_id = "/map";
octomap.header.stamp = ros::Time::now();
bool res = octomap_msgs::fullMapToMsg(*st_tree, octomap);
//octomap.data = td_vector_to_py_list(octomap.data) ;
octmap_pub.publish(octomap) ;
both plugins worked but i didnt fet the 3d construction of the environment why ?? the OccupancyGrid showed a behavier like the laserScan plugin which is points or squares where the laser finds obestacles??
Also, the RVIZ is getting crashed every time, and I get the error "Core damped" ?? Any suggestions ?