31 #include <nav_msgs/GetMap.h> 32 #include <geometry_msgs/Quaternion.h> 33 #include <geometry_msgs/PoseStamped.h> 38 #include <Eigen/Geometry> 56 image_transport_publisher_full_ = image_transport_->advertise(
"map_image/full", 1);
57 image_transport_publisher_tile_ = image_transport_->advertise(
"map_image/tile", 1);
63 cv_img_full_.header.frame_id =
"map_image";
66 cv_img_tile_.header.frame_id =
"map_image";
70 p_size_tiled_map_image_x_ = 64;
71 p_size_tiled_map_image_y_ = 64;
73 ROS_INFO(
"Map to Image node started.");
78 delete image_transport_;
90 int size_x = map->info.width;
91 int size_y = map->info.height;
93 if ((size_x < 3) || (size_y < 3) ){
94 ROS_INFO(
"Map size is only x: %d, y: %d . Not running map to image conversion", size_x, size_y);
99 if (image_transport_publisher_full_.getNumSubscribers() > 0){
100 cv::Mat* map_mat = &cv_img_full_.image;
103 if ( (map_mat->rows != size_y) || (map_mat->cols != size_x)){
104 *map_mat = cv::Mat(size_y, size_x, CV_8U);
107 const std::vector<int8_t>& map_data (map->data);
109 unsigned char *map_mat_data_p=(
unsigned char*) map_mat->data;
112 int size_y_rev = size_y-1;
114 for (
int y = size_y_rev; y >= 0; --y){
116 int idx_map_y = size_x * (size_y_rev -y);
117 int idx_img_y = size_x * y;
119 for (
int x = 0; x < size_x; ++x){
121 int idx = idx_img_y + x;
123 switch (map_data[idx_map_y + x])
126 map_mat_data_p[idx] = 127;
130 map_mat_data_p[idx] = 255;
134 map_mat_data_p[idx] = 0;
139 image_transport_publisher_full_.publish(cv_img_full_.toImageMsg());
143 if ((image_transport_publisher_tile_.getNumSubscribers() > 0) && (pose_ptr_)){
145 world_map_transformer_.setTransforms(*map);
147 Eigen::Vector2f rob_position_world (pose_ptr_->pose.position.x, pose_ptr_->pose.position.y);
148 Eigen::Vector2f rob_position_map (world_map_transformer_.getC2Coords(rob_position_world));
150 Eigen::Vector2i rob_position_mapi (rob_position_map.cast<
int>());
152 Eigen::Vector2i tile_size_lower_halfi (p_size_tiled_map_image_x_ / 2, p_size_tiled_map_image_y_ / 2);
154 Eigen::Vector2i min_coords_map (rob_position_mapi - tile_size_lower_halfi);
157 if (min_coords_map[0] < 0){
158 min_coords_map[0] = 0;
161 if (min_coords_map[1] < 0){
162 min_coords_map[1] = 0;
165 Eigen::Vector2i max_coords_map (min_coords_map + Eigen::Vector2i(p_size_tiled_map_image_x_,p_size_tiled_map_image_y_));
168 if (max_coords_map[0] > size_x){
170 int diff = max_coords_map[0] - size_x;
171 min_coords_map[0] -= diff;
173 max_coords_map[0] = size_x;
176 if (max_coords_map[1] > size_y){
178 int diff = max_coords_map[1] - size_y;
179 min_coords_map[1] -= diff;
181 max_coords_map[1] = size_y;
185 if (min_coords_map[0] < 0){
186 min_coords_map[0] = 0;
189 if (min_coords_map[1] < 0){
190 min_coords_map[1] = 0;
193 Eigen::Vector2i actual_map_dimensions(max_coords_map - min_coords_map);
195 cv::Mat* map_mat = &cv_img_tile_.image;
198 if ( (map_mat->rows != actual_map_dimensions[0]) || (map_mat->cols != actual_map_dimensions[1])){
199 *map_mat = cv::Mat(actual_map_dimensions[0], actual_map_dimensions[1], CV_8U);
202 const std::vector<int8_t>& map_data (map->data);
204 unsigned char *map_mat_data_p=(
unsigned char*) map_mat->data;
207 int y_img = max_coords_map[1]-1;
209 for (
int y = min_coords_map[1]; y < max_coords_map[1];++y){
211 int idx_map_y = y_img-- * size_x;
212 int idx_img_y = (y-min_coords_map[1]) * actual_map_dimensions.x();
214 for (
int x = min_coords_map[0]; x < max_coords_map[0];++x){
216 int img_index = idx_img_y + (x-min_coords_map[0]);
218 switch (map_data[idx_map_y+x])
221 map_mat_data_p[img_index] = 255;
225 map_mat_data_p[img_index] = 127;
229 map_mat_data_p[img_index] = 0;
234 image_transport_publisher_tile_.publish(cv_img_tile_.toImageMsg());
261 int main(
int argc,
char** argv)
263 ros::init(argc, argv,
"map_to_image_node");
geometry_msgs::PoseStampedConstPtr pose_ptr_
int p_size_tiled_map_image_y_
int p_size_tiled_map_image_x_
HectorMapTools::CoordinateTransformer< float > world_map_transformer_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
This node provides occupancy grid maps as images via image_transport, so the transmission consumes le...
ROSCPP_DECL void spin(Spinner &spinner)
cv_bridge::CvImage cv_img_full_
image_transport::Publisher image_transport_publisher_tile_
int main(int argc, char **argv)
image_transport::Publisher image_transport_publisher_full_
image_transport::ImageTransport * image_transport_
ros::Subscriber pose_sub_
void mapCallback(const nav_msgs::OccupancyGridConstPtr &map)
cv_bridge::CvImage cv_img_tile_
void poseCallback(const geometry_msgs::PoseStampedConstPtr &pose)