opencv_demo_node.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
4 #include <cv_bridge/cv_bridge.h>
5 #include <opencv2/highgui/highgui.hpp>
6 
7 using namespace grid_map;
8 using namespace ros;
9 
10 int main(int argc, char** argv)
11 {
12  // Initialize node and publisher.
13  init(argc, argv, "opencv_demo");
14  NodeHandle nodeHandle("~");
15  Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
16  const bool useTransparency = false;
17 
18  // Create grid map.
19  GridMap map({"original", "elevation"});
20  map.setFrameId("map");
21  map.setGeometry(Length(1.2, 2.0), 0.01);
22  ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
23  map.getLength().x(), map.getLength().y(),
24  map.getSize()(0), map.getSize()(1));
25 
26  // Add data.
27  if (!useTransparency) map["original"].setZero();
28  grid_map::Polygon polygon;
29  polygon.setFrameId(map.getFrameId());
30  polygon.addVertex(Position( 0.480, 0.000));
31  polygon.addVertex(Position( 0.164, 0.155));
32  polygon.addVertex(Position( 0.116, 0.500));
33  polygon.addVertex(Position(-0.133, 0.250));
34  polygon.addVertex(Position(-0.480, 0.399));
35  polygon.addVertex(Position(-0.316, 0.000));
36  polygon.addVertex(Position(-0.480, -0.399));
37  polygon.addVertex(Position(-0.133, -0.250));
38  polygon.addVertex(Position( 0.116, -0.500));
39  polygon.addVertex(Position( 0.164, -0.155));
40  polygon.addVertex(Position( 0.480, 0.000));
41 
42  for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) {
43  map.at("original", *iterator) = 0.3;
44  }
45 
46  // Convert to CV image.
47  cv::Mat originalImage;
48  if (useTransparency) {
49  // Note: The template parameters have to be set based on your encoding
50  // of the image. For 8-bit images use `unsigned char`.
51  GridMapCvConverter::toImage<unsigned short, 4>(map, "original", CV_16UC4, 0.0, 0.3, originalImage);
52  } else {
53  GridMapCvConverter::toImage<unsigned short, 1>(map, "original", CV_16UC1, 0.0, 0.3, originalImage);
54  }
55 
56  // Create OpenCV window.
57  cv::namedWindow("OpenCV Demo");
58 
59  // Work with copy of image in a loop.
60  while (nodeHandle.ok()) {
61 
62  // Initialize.
63  ros::Time time = ros::Time::now();
64  map.setTimestamp(time.toNSec());
65  cv::Mat modifiedImage;
66  int blurRadius = 200 - abs((int)(200.0 * sin(time.toSec())));
67  blurRadius = blurRadius - (blurRadius % 2) + 1;
68 
69  // Apply Gaussian blur.
70  cv::GaussianBlur(originalImage, modifiedImage, cv::Size(blurRadius, blurRadius), 0.0, 0.0);
71 
72  // Visualize as image.
73  cv::imshow("OpenCV Demo", modifiedImage);
74  cv::waitKey(40);
75 
76  // Convert resulting image to a grid map.
77  if (useTransparency) {
78  GridMapCvConverter::addLayerFromImage<unsigned short, 4>(modifiedImage, "elevation", map, 0.0, 0.3, 0.3);
79  } else {
80  GridMapCvConverter::addLayerFromImage<unsigned short, 1>(modifiedImage, "elevation", map, 0.0, 0.3);
81  }
82 
83  // Publish grid map.
84  grid_map_msgs::GridMap message;
85  GridMapRosConverter::toMessage(map, message);
86  publisher.publish(message);
87  ROS_INFO_STREAM("Published image and grid map with blur radius " << blurRadius << ".");
88  }
89 
90  return 0;
91 }
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static void toMessage(const grid_map::GridMap &gridMap, grid_map_msgs::GridMap &message)
Node for comparing different normal filters performances.
Eigen::Vector2d Position
#define ROS_INFO(...)
uint64_t toNSec() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void addVertex(const Position &vertex)
#define ROS_INFO_STREAM(args)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
void setFrameId(const std::string &frameId)
static Time now()
bool ok() const
void setFrameId(const std::string &frameId)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Eigen::Array2d Length


grid_map_demos
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:55