MapRegionVisualization.cpp
Go to the documentation of this file.
00001 /*
00002  * MapRegionVisualization.cpp
00003  *
00004  *  Created on: Jun 18, 2014
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_visualization/visualizations/MapRegionVisualization.hpp"
00010 #include <grid_map_visualization/GridMapVisualizationHelpers.hpp>
00011 
00012 // ROS
00013 #include <geometry_msgs/Point.h>
00014 
00015 namespace grid_map_visualization {
00016 
00017 MapRegionVisualization::MapRegionVisualization(ros::NodeHandle& nodeHandle, const std::string& name)
00018     : VisualizationBase(nodeHandle, name),
00019       nVertices_(5),
00020       lineWidth_(0.01)
00021 {
00022 }
00023 
00024 MapRegionVisualization::~MapRegionVisualization()
00025 {
00026 }
00027 
00028 bool MapRegionVisualization::readParameters(XmlRpc::XmlRpcValue& config)
00029 {
00030   VisualizationBase::readParameters(config);
00031 
00032   lineWidth_ = 0.003;
00033   if (!getParam("line_width", lineWidth_)) {
00034     ROS_INFO("MapRegionVisualization with name '%s' did not find a 'line_width' parameter. Using default.", name_.c_str());
00035   }
00036 
00037   int colorValue = 16777215; // white, http://www.wolframalpha.com/input/?i=BitOr%5BBitShiftLeft%5Br%2C16%5D%2C+BitShiftLeft%5Bg%2C8%5D%2C+b%5D+where+%7Br%3D20%2C+g%3D50%2C+b%3D230%7D
00038   if (!getParam("color", colorValue)) {
00039     ROS_INFO("MapRegionVisualization with name '%s' did not find a 'color' parameter. Using default.", name_.c_str());
00040   }
00041   setColorFromColorValue(color_, colorValue, true);
00042 
00043   return true;
00044 }
00045 
00046 bool MapRegionVisualization::initialize()
00047 {
00048   marker_.ns = "map_region";
00049   marker_.lifetime = ros::Duration();
00050   marker_.action = visualization_msgs::Marker::ADD;
00051   marker_.type = visualization_msgs::Marker::LINE_STRIP;
00052   marker_.scale.x = lineWidth_;
00053   marker_.points.resize(nVertices_); // Initialized to [0.0, 0.0, 0.0]
00054   marker_.colors.resize(nVertices_, color_);
00055   publisher_ = nodeHandle_.advertise<visualization_msgs::Marker>(name_, 1, true);
00056   return true;
00057 }
00058 
00059 bool MapRegionVisualization::visualize(const grid_map::GridMap& map)
00060 {
00061   if (!isActive()) return true;
00062 
00063   // TODO Replace this with ploygon?
00064 
00065   // Set marker info.
00066   marker_.header.frame_id = map.getFrameId();
00067   marker_.header.stamp.fromNSec(map.getTimestamp());
00068 
00069   // Adapt positions of markers.
00070   float halfLengthX = map.getLength().x() / 2.0;
00071   float halfLengthY = map.getLength().y() / 2.0;
00072 
00073   marker_.points[0].x = map.getPosition().x() + halfLengthX;
00074   marker_.points[0].y = map.getPosition().y() + halfLengthY;
00075   marker_.points[1].x = map.getPosition().x() + halfLengthX;
00076   marker_.points[1].y = map.getPosition().y() - halfLengthY;
00077   marker_.points[2].x = map.getPosition().x() - halfLengthX;
00078   marker_.points[2].y = map.getPosition().y() - halfLengthY;
00079   marker_.points[3].x = map.getPosition().x() - halfLengthX;
00080   marker_.points[3].y = map.getPosition().y() + halfLengthY;
00081   marker_.points[4].x = marker_.points[0].x;
00082   marker_.points[4].y = marker_.points[0].y;
00083 
00084   publisher_.publish(marker_);
00085   return true;
00086 }
00087 
00088 } /* namespace */


grid_map_visualization
Author(s): Péter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:50