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 "ros/ros.h"
00038 #include "std_msgs/String.h"
00039 #include "std_msgs/Int64MultiArray.h"
00040 #include "std_msgs/MultiArrayLayout.h"
00041 #include "std_msgs/MultiArrayDimension.h"
00042 #include "nav_msgs/OccupancyGrid.h"
00043 #include "map_compressor/CompressedOccupancyGrid.h"
00044 #include <vector>
00045 #include <sstream>
00046
00047 ros::Publisher comp_map_pub;
00048
00049 void compressCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
00050 {
00051 ROS_INFO("Map width: [%d]\n Map height: [%d] ", msg->info.width, msg->info.height);
00052 uint32_t w = msg->info.width;
00053 uint32_t h = msg->info.height;
00054 uint32_t s_uncomp = w*h;
00055 uint32_t c0 = 0;
00056 uint32_t c1 = 0;
00057 uint32_t c2 = 0;
00058 int8_t current_val = msg->data[0];
00059 int8_t prev_val = msg->data[0];
00060 uint64_t count = 0;
00061 int num_switch = 0;
00062 std::vector<int64_t> compressed_data;
00063 map_compressor::CompressedOccupancyGrid compressed_map;
00064
00065 for(uint32_t i=0; i<s_uncomp; i++){
00066
00067 current_val = msg->data[i];
00068
00069 if(current_val == -1)c0++;
00070 else if(current_val == 100)c1++;
00071 else if(current_val == 0)c2++;
00072
00073 if(current_val != prev_val){
00074 num_switch++;
00075
00076
00077 compressed_data.push_back(count);
00078 compressed_data.push_back((int64_t)prev_val);
00079
00080 count=0;
00081 prev_val = current_val;
00082 }
00083
00084 count++;
00085 }
00086
00087
00088 compressed_map.header = msg->header;
00089 compressed_map.info = msg->info;
00090 compressed_map.data = compressed_data;
00091
00092 ROS_INFO("Map histogram:\n[%d] -1s,\n[%d] 100s,\n[%d] 0s ", c0, c1, c2);
00093
00094 ROS_INFO("Map size before compression: %d\n", s_uncomp);
00095
00096 ROS_INFO("Grid value changed %d times\n", (int)(compressed_data.size()/2));
00097
00098
00099
00100
00101
00102 comp_map_pub.publish(compressed_map);
00103
00104 }
00105
00106 int main(int argc, char **argv)
00107 {
00108 ros::init(argc, argv, "map_compressor");
00109 ros::NodeHandle n;
00110 ros::Subscriber map_sub = n.subscribe("map",1,compressCallback);
00111 comp_map_pub = n.advertise<map_compressor::CompressedOccupancyGrid>("compressed_map",1);
00112 ros::spin();
00113 return 0;
00114 }