Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <boost/shared_ptr.hpp>
00033 #include <ros/ros.h>
00034 #include <tf/tf.h>
00035 #include <tf/transform_listener.h>
00036 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
00037 #include <octomap_msgs/conversions.h>
00038
00039 static void publishOctomap(ros::Publisher *octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor *server)
00040 {
00041 octomap_msgs::Octomap map;
00042
00043 map.header.frame_id = server->getMapFrame();
00044 map.header.stamp = ros::Time::now();
00045
00046 server->getOcTreePtr()->lockRead();
00047 try
00048 {
00049 if (!octomap_msgs::binaryMapToMsgData(*server->getOcTreePtr(), map.data))
00050 ROS_ERROR_THROTTLE(1, "Could not generate OctoMap message");
00051 }
00052 catch(...)
00053 {
00054 ROS_ERROR_THROTTLE(1, "Exception thrown while generating OctoMap message");
00055 }
00056 server->getOcTreePtr()->unlockRead();
00057
00058 octree_binary_pub->publish(map);
00059 }
00060
00061 int main(int argc, char **argv)
00062 {
00063 ros::init(argc, argv, "occupancy_map_server");
00064 ros::NodeHandle nh;
00065 ros::Publisher octree_binary_pub = nh.advertise<octomap_msgs::Octomap>("octomap_binary", 1);
00066 boost::shared_ptr<tf::Transformer> listener = boost::make_shared<tf::TransformListener>(ros::Duration(5.0));
00067 occupancy_map_monitor::OccupancyMapMonitor server(listener);
00068 server.setUpdateCallback(boost::bind(&publishOctomap, &octree_binary_pub, &server));
00069 server.startMonitor();
00070
00071 ros::spin();
00072 return 0;
00073 }