MapRegionVisualization.cpp
Go to the documentation of this file.
1 /*
2  * MapRegionVisualization.cpp
3  *
4  * Created on: Jun 18, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
11 
12 // ROS
13 #include <geometry_msgs/Point.h>
14 
15 namespace grid_map_visualization {
16 
18  : VisualizationBase(nodeHandle, name),
19  nVertices_(5),
20  lineWidth_(0.01)
21 {
22 }
23 
25 {
26 }
27 
29 {
31 
32  lineWidth_ = 0.003;
33  if (!getParam("line_width", lineWidth_)) {
34  ROS_INFO("MapRegionVisualization with name '%s' did not find a 'line_width' parameter. Using default.", name_.c_str());
35  }
36 
37  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
38  if (!getParam("color", colorValue)) {
39  ROS_INFO("MapRegionVisualization with name '%s' did not find a 'color' parameter. Using default.", name_.c_str());
40  }
41  setColorFromColorValue(color_, colorValue, true);
42 
43  return true;
44 }
45 
47 {
48  marker_.ns = "map_region";
49  marker_.lifetime = ros::Duration();
50  marker_.action = visualization_msgs::Marker::ADD;
51  marker_.type = visualization_msgs::Marker::LINE_STRIP;
52  marker_.scale.x = lineWidth_;
53  marker_.points.resize(nVertices_); // Initialized to [0.0, 0.0, 0.0]
54  marker_.colors.resize(nVertices_, color_);
55  publisher_ = nodeHandle_.advertise<visualization_msgs::Marker>(name_, 1, true);
56  return true;
57 }
58 
60 {
61  if (!isActive()) return true;
62 
63  // TODO Replace this with ploygon?
64 
65  // Set marker info.
66  marker_.header.frame_id = map.getFrameId();
67  marker_.header.stamp.fromNSec(map.getTimestamp());
68 
69  // Adapt positions of markers.
70  float halfLengthX = map.getLength().x() / 2.0;
71  float halfLengthY = map.getLength().y() / 2.0;
72 
73  marker_.points[0].x = map.getPosition().x() + halfLengthX;
74  marker_.points[0].y = map.getPosition().y() + halfLengthY;
75  marker_.points[1].x = map.getPosition().x() + halfLengthX;
76  marker_.points[1].y = map.getPosition().y() - halfLengthY;
77  marker_.points[2].x = map.getPosition().x() - halfLengthX;
78  marker_.points[2].y = map.getPosition().y() - halfLengthY;
79  marker_.points[3].x = map.getPosition().x() - halfLengthX;
80  marker_.points[3].y = map.getPosition().y() + halfLengthY;
81  marker_.points[4].x = marker_.points[0].x;
82  marker_.points[4].y = marker_.points[0].y;
83 
85  return true;
86 }
87 
88 } /* namespace */
const Length & getLength() const
Time getTimestamp() const
void publish(const boost::shared_ptr< M > &message) const
bool getPosition(const Index &index, Position &position) const
MapRegionVisualization(ros::NodeHandle &nodeHandle, const std::string &name)
const std::string & getFrameId() const
const unsigned int nVertices_
Number of vertices of the map region visualization.
bool getParam(const std::string &name, std::string &value)
visualization_msgs::Marker marker_
Marker to be published.
#define ROS_INFO(...)
std_msgs::ColorRGBA color_
Color of the map region visualization.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double lineWidth_
Line width of the map region marker [m].
ros::NodeHandle & nodeHandle_
ROS nodehandle.
std::string name_
Name of the visualization.
void setColorFromColorValue(std_msgs::ColorRGBA &color, const unsigned long &colorValue, bool resetTransparency=true)
ros::Publisher publisher_
ROS publisher of the occupancy grid.
virtual bool readParameters(XmlRpc::XmlRpcValue &config)


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