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
00033
00034
00035
00036
00037 #include <boost/shared_ptr.hpp>
00038 #include <ros/ros.h>
00039 #include <tf/tf.h>
00040 #include <tf/transform_listener.h>
00041 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
00042 #include <octomap_msgs/conversions.h>
00043
00044 static void publishOctomap(ros::Publisher *octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor *server)
00045 {
00046 octomap_msgs::Octomap map;
00047
00048 map.header.frame_id = server->getMapFrame();
00049 map.header.stamp = ros::Time::now();
00050
00051 server->getOcTreePtr()->lockRead();
00052 try
00053 {
00054 if (!octomap_msgs::binaryMapToMsgData(*server->getOcTreePtr(), map.data))
00055 ROS_ERROR_THROTTLE(1, "Could not generate OctoMap message");
00056 }
00057 catch(...)
00058 {
00059 ROS_ERROR_THROTTLE(1, "Exception thrown while generating OctoMap message");
00060 }
00061 server->getOcTreePtr()->unlockRead();
00062
00063 octree_binary_pub->publish(map);
00064 }
00065
00066 int main(int argc, char **argv)
00067 {
00068 ros::init(argc, argv, "occupancy_map_server");
00069 ros::NodeHandle nh;
00070 ros::Publisher octree_binary_pub = nh.advertise<octomap_msgs::Octomap>("octomap_binary", 1);
00071 boost::shared_ptr<tf::Transformer> listener = boost::make_shared<tf::TransformListener>(ros::Duration(5.0));
00072 occupancy_map_monitor::OccupancyMapMonitor server(listener);
00073 server.setUpdateCallback(boost::bind(&publishOctomap, &octree_binary_pub, &server));
00074 server.startMonitor();
00075
00076 ros::spin();
00077 return 0;
00078 }