VectorVisualization.cpp
Go to the documentation of this file.
1 /*
2  * VectorVisualization.cpp
3  *
4  * Created on: Sep 16, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
11 // Color conversion
13 
14 // Iterator
16 
17 // ROS
18 #include <geometry_msgs/Point.h>
19 #include <geometry_msgs/Vector3.h>
20 
21 namespace grid_map_visualization {
22 
23 VectorVisualization::VectorVisualization(ros::NodeHandle& nodeHandle, const std::string& name)
24  : VisualizationBase(nodeHandle, name)
25 {
26 }
27 
29 {
30 }
31 
33 {
35 
36  std::string typePrefix;
37  if (!getParam("layer_prefix", typePrefix)) {
38  ROS_ERROR("VectorVisualization with name '%s' did not find a 'layer_prefix' parameter.", name_.c_str());
39  return false;
40  }
41  types_.push_back(typePrefix + "x");
42  types_.push_back(typePrefix + "y");
43  types_.push_back(typePrefix + "z");
44 
45  if (!getParam("position_layer", positionLayer_)) {
46  ROS_ERROR("VectorVisualization with name '%s' did not find a 'position_layer' parameter.", name_.c_str());
47  return false;
48  }
49 
50  scale_ = 1.0;
51  if (!getParam("scale", scale_)) {
52  ROS_INFO("VectorVisualization with name '%s' did not find a 'scale' parameter. Using default.", name_.c_str());
53  }
54 
55  lineWidth_ = 0.003;
56  if (!getParam("line_width", lineWidth_)) {
57  ROS_INFO("VectorVisualization with name '%s' did not find a 'line_width' parameter. Using default.", name_.c_str());
58  }
59 
60  int colorValue = 65280; // green
61  if (!getParam("color", colorValue)) {
62  ROS_INFO("VectorVisualization with name '%s' did not find a 'color' parameter. Using default.", name_.c_str());
63  }
64  setColorFromColorValue(color_, colorValue, true);
65 
66  return true;
67 }
68 
70 {
71  marker_.ns = "vector";
72  marker_.lifetime = ros::Duration();
73  marker_.action = visualization_msgs::Marker::ADD;
74  marker_.type = visualization_msgs::Marker::LINE_LIST;
75  marker_.scale.x = lineWidth_;
76  publisher_ = nodeHandle_.advertise<visualization_msgs::Marker>(name_, 1, true);
77  return true;
78 }
79 
81 {
82  if (!isActive()) return true;
83 
84  for (const auto& type : types_) {
85  if (!map.exists(type)) {
87  "VectorVisualization::visualize: No grid map layer with name '" << type << "' found.");
88  return false;
89  }
90  }
91 
92  // Set marker info.
93  marker_.header.frame_id = map.getFrameId();
94  marker_.header.stamp.fromNSec(map.getTimestamp());
95 
96  // Clear points.
97  marker_.points.clear();
98  marker_.colors.clear();
99 
100  for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
101  {
102  if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue;
103  geometry_msgs::Vector3 vector;
104  vector.x = map.at(types_[0], *iterator);
105  vector.y = map.at(types_[1], *iterator);
106  vector.z = map.at(types_[2], *iterator);
107 
108  Eigen::Vector3d position;
109  map.getPosition3(positionLayer_, *iterator, position);
110  geometry_msgs::Point startPoint;
111  startPoint.x = position.x();
112  startPoint.y = position.y();
113  startPoint.z = position.z();
114  marker_.points.push_back(startPoint);
115 
116  geometry_msgs::Point endPoint;
117  endPoint.x = startPoint.x + scale_ * vector.x;
118  endPoint.y = startPoint.y + scale_ * vector.y;
119  endPoint.z = startPoint.z + scale_ * vector.z;
120  marker_.points.push_back(endPoint);
121 
122  marker_.colors.push_back(color_); // Each vertex needs a color.
123  marker_.colors.push_back(color_);
124  }
125 
127  return true;
128 }
129 
130 } /* namespace */
VectorVisualization(ros::NodeHandle &nodeHandle, const std::string &name)
double lineWidth_
Width of the line markers [m].
Time getTimestamp() const
void publish(const boost::shared_ptr< M > &message) const
const std::string & getFrameId() const
std::vector< std::string > types_
Types that are transformed to vectors.
bool isValid(const Index &index) const
bool exists(const std::string &layer) const
bool visualize(const grid_map::GridMap &map)
visualization_msgs::Marker marker_
Marker to be published.
bool getParam(const std::string &name, std::string &value)
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std_msgs::ColorRGBA color_
Color of the vectors.
#define ROS_WARN_STREAM(args)
float & at(const std::string &layer, const Index &index)
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.
bool readParameters(XmlRpc::XmlRpcValue &config)
virtual bool readParameters(XmlRpc::XmlRpcValue &config)
#define ROS_ERROR(...)
bool getPosition3(const std::string &layer, const Index &index, Position3 &position) const
std::string positionLayer_
Type that is the position of the vectors.


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