ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

how to create octomap_masgs for octomap_rviz_plugins

asked 2015-02-17 00:57:51 -0600

RSA_kustar gravatar image

updated 2015-02-17 05:05:59 -0600

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 ?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-02-17 04:01:01 -0600

dornhege gravatar image

The header of your octomap message is empty. It must be relative to some frame.

edit flag offensive delete link more

Comments

I added octmap.frameid = "/map" but i didnt get a 3d structuring of the environment.. it gave me squares like laserScan plugin dose. Also, RVIZ is getting crashed and I get "core damped each time..

I will update my question with these information

RSA_kustar gravatar image RSA_kustar  ( 2015-02-17 04:53:42 -0600 )edit

Sounds correct to me. An octomap is drawn by many cubes.

dornhege gravatar image dornhege  ( 2015-02-17 05:55:34 -0600 )edit

yes but rviz is getting crashed everytime .. could you please check the last edit I did in the question

RSA_kustar gravatar image RSA_kustar  ( 2015-02-17 07:21:21 -0600 )edit

Randomly or every time it gets one of your messages? Does the octree that you save look correct in octovis?

dornhege gravatar image dornhege  ( 2015-02-17 07:47:25 -0600 )edit

It always get stuck and I didnt check the octovis yet

RSA_kustar gravatar image RSA_kustar  ( 2015-02-18 00:11:49 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2015-02-17 00:57:51 -0600

Seen: 2,044 times

Last updated: Feb 17 '15