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 #include "ros/ros.h"
00030
00031 #include <nav_msgs/GetMap.h>
00032 #include <geometry_msgs/Quaternion.h>
00033 #include <geometry_msgs/PoseStamped.h>
00034 #include <sensor_msgs/image_encodings.h>
00035
00036 #include <image_transport/image_transport.h>
00037 #include <Eigen/Geometry>
00038
00039 #include <HectorMapTools.h>
00040 #include <math.h>
00041 #include <tf/transform_datatypes.h>
00042
00043 using namespace std;
00044
00045 double resolution = 0.05;
00046
00050 class MapAsImageProvider
00051 {
00052 public:
00053 MapAsImageProvider()
00054 : pn_("~")
00055 {
00056
00057 image_transport_ = new image_transport::ImageTransport(n_);
00058 image_transport_subscriber_map = image_transport_->subscribe("map_image_raw", 1, &MapAsImageProvider::mapCallback,this);
00059 map_publisher = n_.advertise<nav_msgs::OccupancyGrid>("/map_from_jpeg", 50);
00060
00061
00062 ROS_INFO("Image to Map node started.");
00063 }
00064
00065 ~MapAsImageProvider()
00066 {
00067 delete image_transport_;
00068 }
00069
00070
00071
00072 void mapCallback(const sensor_msgs::ImageConstPtr& image)
00073 {
00074
00075 nav_msgs::OccupancyGrid map;
00076 map.header.stamp = image->header.stamp;
00077 map.header.frame_id = "map";
00078 map.info.width = image->width;
00079 map.info.height = image->height;
00080 map.info.origin.orientation.w = 1;
00081 map.info.resolution = resolution;
00082 map.info.origin.position.x = -((image->width + 1) * map.info.resolution * 0.5f);
00083 map.info.origin.position.y = -((image->height + 1) * map.info.resolution * 0.5f);
00084
00085 int data;
00086 for(int i=image->height -1; i>=0; i--)
00087 {
00088 for (unsigned int j=0; j<image->width;j++)
00089 {
00090 data = image->data[i*image->width + j];
00091 if(data >=123 && data <= 131){
00092 map.data.push_back(-1);
00093 }
00094 else if (data >=251 && data <= 259){
00095 map.data.push_back(0);
00096 }
00097 else
00098 map.data.push_back(100);
00099 }
00100 }
00101 map_publisher.publish(map);
00102 }
00103
00104 ros::Publisher map_publisher;
00105
00106
00107 image_transport::Subscriber image_transport_subscriber_map;
00108
00109 image_transport::ImageTransport* image_transport_;
00110
00111 ros::NodeHandle n_;
00112 ros::NodeHandle pn_;
00113
00114 };
00115
00116 int main(int argc, char** argv)
00117 {
00118 ros::init(argc, argv, "image_to_map_node");
00119
00120 ros::NodeHandle nh("~");
00121 nh.param("resolution", resolution, 0.05);
00122
00123 MapAsImageProvider map_image_provider;
00124
00125 ros::spin();
00126
00127 return 0;
00128 }