tutorial_demo_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <grid_map_ros/grid_map_ros.hpp>
00003 #include <vector>
00004 #include <string>
00005 #include <cmath>
00006 #include <limits>
00007 
00008 using namespace grid_map;
00009 
00010 int main(int argc, char** argv)
00011 {
00012   // Initialize node and publisher.
00013   ros::init(argc, argv, "grid_map_tutorial_demo");
00014   ros::NodeHandle nh("~");
00015   ros::Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
00016 
00017   // Create grid map.
00018   GridMap map({"elevation", "normal_x", "normal_y", "normal_z"});
00019   map.setFrameId("map");
00020   map.setGeometry(Length(1.2, 2.0), 0.03, Position(0.0, -0.1));
00021   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.",
00022     map.getLength().x(), map.getLength().y(),
00023     map.getSize()(0), map.getSize()(1),
00024     map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str());
00025 
00026   // Work with grid map in a loop.
00027   ros::Rate rate(30.0);
00028   while (nh.ok()) {
00029     ros::Time time = ros::Time::now();
00030 
00031     // Add elevation and surface normal (iterating through grid map and adding data).
00032     for (GridMapIterator it(map); !it.isPastEnd(); ++it) {
00033       Position position;
00034       map.getPosition(*it, position);
00035       map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x();
00036       Eigen::Vector3d normal(-0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()),
00037                              -position.x() * std::cos(3.0 * time.toSec() + 5.0 * position.y()), 1.0);
00038       normal.normalize();
00039       map.at("normal_x", *it) = normal.x();
00040       map.at("normal_y", *it) = normal.y();
00041       map.at("normal_z", *it) = normal.z();
00042     }
00043 
00044     // Add noise (using Eigen operators).
00045     map.add("noise", 0.015 * Matrix::Random(map.getSize()(0), map.getSize()(1)));
00046     map.add("elevation_noisy", map.get("elevation") + map["noise"]);
00047 
00048     // Adding outliers (accessing cell by position).
00049     for (unsigned int i = 0; i < 500; ++i) {
00050       Position randomPosition = Position::Random();
00051       if (map.isInside(randomPosition))
00052         map.atPosition("elevation_noisy", randomPosition) = std::numeric_limits<float>::infinity();
00053     }
00054 
00055     // Filter values for submap (iterators).
00056     map.add("elevation_filtered", map.get("elevation_noisy"));
00057     Position topLeftCorner(1.0, 0.4);
00058     boundPositionToRange(topLeftCorner, map.getLength(), map.getPosition());
00059     Index startIndex;
00060     map.getIndex(topLeftCorner, startIndex);
00061     ROS_INFO_ONCE("Top left corner was limited from (1.0, 0.2) to (%f, %f) and corresponds to index (%i, %i).",
00062              topLeftCorner.x(), topLeftCorner.y(), startIndex(0), startIndex(1));
00063 
00064     Size size = (Length(1.2, 0.8) / map.getResolution()).cast<int>();
00065     SubmapIterator it(map, startIndex, size);
00066     for (; !it.isPastEnd(); ++it) {
00067       Position currentPosition;
00068       map.getPosition(*it, currentPosition);
00069       double radius = 0.1;
00070       double mean = 0.0;
00071       double sumOfWeights = 0.0;
00072 
00073       // Compute weighted mean.
00074       for (CircleIterator circleIt(map, currentPosition, radius);
00075           !circleIt.isPastEnd(); ++circleIt) {
00076         if (!map.isValid(*circleIt, "elevation_noisy")) continue;
00077         Position currentPositionInCircle;
00078         map.getPosition(*circleIt, currentPositionInCircle);
00079 
00080         // Computed weighted mean based on Euclidian distance.
00081         double distance = (currentPosition - currentPositionInCircle).norm();
00082         double weight = pow(radius - distance, 2);
00083         mean += weight * map.at("elevation_noisy", *circleIt);
00084         sumOfWeights += weight;
00085       }
00086 
00087       map.at("elevation_filtered", *it) = mean / sumOfWeights;
00088     }
00089 
00090     // Show absolute difference and compute mean squared error.
00091     map.add("error", (map.get("elevation_filtered") - map.get("elevation")).cwiseAbs());
00092     unsigned int nCells = map.getSize().prod();
00093     // cppcheck-suppress unreadVariable
00094     double rootMeanSquaredError = sqrt((map["error"].array().pow(2).sum()) / nCells);
00095 
00096     // Publish grid map.
00097     map.setTimestamp(time.toNSec());
00098     grid_map_msgs::GridMap message;
00099     GridMapRosConverter::toMessage(map, message);
00100     publisher.publish(message);
00101     ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec());
00102 
00103     rate.sleep();
00104   }
00105 
00106   return 0;
00107 }


grid_map_demos
Author(s): P├ęter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:58