tutorial_demo_node.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
3 #include <vector>
4 #include <string>
5 #include <cmath>
6 #include <limits>
7 
8 using namespace grid_map;
9 
10 int main(int argc, char** argv)
11 {
12  // Initialize node and publisher.
13  ros::init(argc, argv, "grid_map_tutorial_demo");
14  ros::NodeHandle nh("~");
15  ros::Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
16 
17  // Create grid map.
18  GridMap map({"elevation", "normal_x", "normal_y", "normal_z"});
19  map.setFrameId("map");
20  map.setGeometry(Length(1.2, 2.0), 0.03, Position(0.0, -0.1));
21  ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.",
22  map.getLength().x(), map.getLength().y(),
23  map.getSize()(0), map.getSize()(1),
24  map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str());
25 
26  // Work with grid map in a loop.
27  ros::Rate rate(30.0);
28  while (nh.ok()) {
29  ros::Time time = ros::Time::now();
30 
31  // Add elevation and surface normal (iterating through grid map and adding data).
32  for (GridMapIterator it(map); !it.isPastEnd(); ++it) {
33  Position position;
34  map.getPosition(*it, position);
35  map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x();
36  Eigen::Vector3d normal(-0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()),
37  -position.x() * std::cos(3.0 * time.toSec() + 5.0 * position.y()), 1.0);
38  normal.normalize();
39  map.at("normal_x", *it) = normal.x();
40  map.at("normal_y", *it) = normal.y();
41  map.at("normal_z", *it) = normal.z();
42  }
43 
44  // Add noise (using Eigen operators).
45  map.add("noise", 0.015 * Matrix::Random(map.getSize()(0), map.getSize()(1)));
46  map.add("elevation_noisy", map.get("elevation") + map["noise"]);
47 
48  // Adding outliers (accessing cell by position).
49  for (unsigned int i = 0; i < 500; ++i) {
50  Position randomPosition = Position::Random();
51  if (map.isInside(randomPosition))
52  map.atPosition("elevation_noisy", randomPosition) = std::numeric_limits<float>::infinity();
53  }
54 
55  // Filter values for submap (iterators).
56  map.add("elevation_filtered", map.get("elevation_noisy"));
57  Position topLeftCorner(1.0, 0.4);
58  boundPositionToRange(topLeftCorner, map.getLength(), map.getPosition());
59  Index startIndex;
60  map.getIndex(topLeftCorner, startIndex);
61  ROS_INFO_ONCE("Top left corner was limited from (1.0, 0.2) to (%f, %f) and corresponds to index (%i, %i).",
62  topLeftCorner.x(), topLeftCorner.y(), startIndex(0), startIndex(1));
63 
64  Size size = (Length(1.2, 0.8) / map.getResolution()).cast<int>();
65  SubmapIterator it(map, startIndex, size);
66  for (; !it.isPastEnd(); ++it) {
67  Position currentPosition;
68  map.getPosition(*it, currentPosition);
69  double radius = 0.1;
70  double mean = 0.0;
71  double sumOfWeights = 0.0;
72 
73  // Compute weighted mean.
74  for (CircleIterator circleIt(map, currentPosition, radius); !circleIt.isPastEnd(); ++circleIt) {
75  if (!map.isValid(*circleIt, "elevation_noisy")) continue;
76  Position currentPositionInCircle;
77  map.getPosition(*circleIt, currentPositionInCircle);
78 
79  // Computed weighted mean based on Euclidian distance.
80  double distance = (currentPosition - currentPositionInCircle).norm();
81  double weight = pow(radius - distance, 2);
82  mean += weight * map.at("elevation_noisy", *circleIt);
83  sumOfWeights += weight;
84  }
85 
86  map.at("elevation_filtered", *it) = mean / sumOfWeights;
87  }
88 
89  // Show absolute difference and compute mean squared error.
90  map.add("error", (map.get("elevation_filtered") - map.get("elevation")).cwiseAbs());
91 
92  // Publish grid map.
93  map.setTimestamp(time.toNSec());
94  grid_map_msgs::GridMap message;
95  GridMapRosConverter::toMessage(map, message);
96  publisher.publish(message);
97  ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec());
98 
99  rate.sleep();
100  }
101 
102  return 0;
103 }
Eigen::Array2i Index
void publish(const boost::shared_ptr< M > &message) const
#define ROS_INFO_ONCE(...)
Eigen::Array2i Size
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)
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
Node for comparing different normal filters performances.
Eigen::Vector2d Position
#define ROS_INFO(...)
uint64_t toNSec() const
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
static Time now()
#define ROS_INFO_THROTTLE(rate,...)
bool ok() const
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
void setFrameId(const std::string &frameId)
Eigen::Array2d Length


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