Ask Your Question
0

How can I add a floorplan image in rviz?

asked 2012-11-16 09:04:58 -0500

I would like to be able to add a floorplan image in rviz simply to debug localization. My map is feature-based, so the image would have no other purpose than to make visualization richer.

Is it possible to obtain a result similar to what is observed with the DisplayTypes/Map without having to publish nav_msgs/OccupancyGrid messages? If not, could anyone give me any pointers as to how I would be able to write such a rviz plugin myself?

edit retag flag offensive close merge delete

4 Answers

Sort by ยป oldest newest most voted
1

answered 2012-11-16 09:49:01 -0500

This appears to be related to this question

I suppose you could publish a rviz display marker with a custom texture.

edit flag offensive delete link more

Comments

This looks like it could work. I will try that and accept your answer if I have good results.

georgebrindeiro gravatar image georgebrindeiro  ( 2012-11-17 09:13:45 -0500 )edit
1

answered 2012-11-16 09:52:40 -0500

John Hoare gravatar image

I know I'm not answering your direct question, but couldn't you use the map_server (http://ros.org/wiki/map_server) node, remap /map to something else like /map_vis, and then use RViz to just visualize that map without interfering with anything else in your system?

edit flag offensive delete link more

Comments

Interesting solution! I wouldn't really want to have map_server to interpret occupancy values by thresholding the image though, since there are a few things that are written/drawn on it that are not physical objects. I'm sure this will help others, though!

georgebrindeiro gravatar image georgebrindeiro  ( 2012-11-17 09:10:21 -0500 )edit
0

answered 2017-01-15 19:03:58 -0500

mayuzumi gravatar image

after trying various ways, i found that publishing the image as colored pointcloud is actually a very good approach.

edit flag offensive delete link more
0

answered 2016-10-11 11:04:50 -0500

I wrote a function using opencv in order to do so.

LUcorner is the position of the upper-left corner of the image in rviz. sizeSq is a scaling factor.

void sendImageToRviz(std::string frame_id, int id, ros::Publisher* marker_pub, cv::Mat src, double sizeSq, std::pair<double, double=""> LUcorner) {

visualization_msgs::Marker image;
image.header.frame_id = frame_id;
image.header.stamp = ros::Time::now();
image.ns = "image";
image.id = id;
image.action = visualization_msgs::Marker::ADD;
image.type = visualization_msgs::Marker::TRIANGLE_LIST;
image.scale.x = 1;
image.scale.y = 1;
image.scale.z = 1;

double pix = sizeSq / src.rows;
    geometry_msgs::Point p;
std_msgs::ColorRGBA crgb;

for(int r = 0; r < src.rows; ++r) {
    for(int c = 0; c < src.rows; ++c) {
      cv::Vec3b intensity = src.at<cv::Vec3b>(r, c);
      crgb.r = intensity.val[2] / 255.0;
      crgb.g = intensity.val[1] / 255.0;
      crgb.b = intensity.val[0] / 255.0;
      crgb.a = 1.0;

      p.z = 0;
      p.x = LUcorner.first + r * pix;
      p.y = LUcorner.second + c * pix;
      image.points.push_back(p);
      image.colors.push_back(crgb);
      p.x = LUcorner.first + (r + 1) * pix;
      p.y = LUcorner.second + c * pix;
      image.points.push_back(p);
      image.colors.push_back(crgb);
      p.x = LUcorner.first + r * pix;
      p.y = LUcorner.second + (c + 1) * pix;
      image.points.push_back(p);
      image.colors.push_back(crgb);
      p.x = LUcorner.first + (r + 1) * pix;
      p.y = LUcorner.second + c * pix;
      image.points.push_back(p);
      image.colors.push_back(crgb);
      p.x = LUcorner.first + (r + 1) * pix;
      p.y = LUcorner.second + (c + 1) * pix;
      image.points.push_back(p);
      image.colors.push_back(crgb);
      p.x = LUcorner.first + r * pix;
      p.y = LUcorner.second + (c + 1) * pix;
      image.points.push_back(p);
      image.colors.push_back(crgb);
    }
}
marker_pub->publish(image);

}

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2012-11-16 09:04:58 -0500

Seen: 3,897 times

Last updated: Oct 11 '16