VectorVisualization.cpp
Go to the documentation of this file.
00001 /*
00002  * VectorVisualization.cpp
00003  *
00004  *  Created on: Sep 16, 2014
00005  *      Author: Péter Fankhauser
00006  *   Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_visualization/visualizations/VectorVisualization.hpp"
00010 
00011 // Color conversion
00012 #include <grid_map_visualization/GridMapVisualizationHelpers.hpp>
00013 
00014 // Iterator
00015 #include <grid_map_core/iterators/GridMapIterator.hpp>
00016 
00017 // ROS
00018 #include <geometry_msgs/Point.h>
00019 #include <geometry_msgs/Vector3.h>
00020 
00021 namespace grid_map_visualization {
00022 
00023 VectorVisualization::VectorVisualization(ros::NodeHandle& nodeHandle, const std::string& name)
00024     : VisualizationBase(nodeHandle, name)
00025 {
00026 }
00027 
00028 VectorVisualization::~VectorVisualization()
00029 {
00030 }
00031 
00032 bool VectorVisualization::readParameters(XmlRpc::XmlRpcValue& config)
00033 {
00034   VisualizationBase::readParameters(config);
00035 
00036   std::string typePrefix;
00037   if (!getParam("layer_prefix", typePrefix)) {
00038     ROS_ERROR("VectorVisualization with name '%s' did not find a 'layer_prefix' parameter.", name_.c_str());
00039     return false;
00040   }
00041   types_.push_back(typePrefix + "x");
00042   types_.push_back(typePrefix + "y");
00043   types_.push_back(typePrefix + "z");
00044 
00045   if (!getParam("position_layer", positionLayer_)) {
00046     ROS_ERROR("VectorVisualization with name '%s' did not find a 'position_layer' parameter.", name_.c_str());
00047     return false;
00048   }
00049 
00050   scale_ = 1.0;
00051   if (!getParam("scale", scale_)) {
00052     ROS_INFO("VectorVisualization with name '%s' did not find a 'scale' parameter. Using default.", name_.c_str());
00053   }
00054 
00055   lineWidth_ = 0.003;
00056   if (!getParam("line_width", lineWidth_)) {
00057     ROS_INFO("VectorVisualization with name '%s' did not find a 'line_width' parameter. Using default.", name_.c_str());
00058   }
00059 
00060   int colorValue = 65280; // green
00061   if (!getParam("color", colorValue)) {
00062     ROS_INFO("VectorVisualization with name '%s' did not find a 'color' parameter. Using default.", name_.c_str());
00063   }
00064   setColorFromColorValue(color_, colorValue, true);
00065 
00066   return true;
00067 }
00068 
00069 bool VectorVisualization::initialize()
00070 {
00071   marker_.ns = "vector";
00072   marker_.lifetime = ros::Duration();
00073   marker_.action = visualization_msgs::Marker::ADD;
00074   marker_.type = visualization_msgs::Marker::LINE_LIST;
00075   marker_.scale.x = lineWidth_;
00076   publisher_ = nodeHandle_.advertise<visualization_msgs::Marker>(name_, 1, true);
00077   return true;
00078 }
00079 
00080 bool VectorVisualization::visualize(const grid_map::GridMap& map)
00081 {
00082   if (!isActive()) return true;
00083 
00084   for (const auto& type : types_) {
00085     if (!map.exists(type)) {
00086       ROS_WARN_STREAM(
00087           "VectorVisualization::visualize: No grid map layer with name '" << type << "' found.");
00088       return false;
00089     }
00090   }
00091 
00092   // Set marker info.
00093   marker_.header.frame_id = map.getFrameId();
00094   marker_.header.stamp.fromNSec(map.getTimestamp());
00095 
00096   // Clear points.
00097   marker_.points.clear();
00098   marker_.colors.clear();
00099 
00100   for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
00101   {
00102     if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue;
00103     geometry_msgs::Vector3 vector;
00104     vector.x = map.at(types_[0], *iterator);
00105     vector.y = map.at(types_[1], *iterator);
00106     vector.z = map.at(types_[2], *iterator);
00107 
00108     Eigen::Vector3d position;
00109     map.getPosition3(positionLayer_, *iterator, position);
00110     geometry_msgs::Point startPoint;
00111     startPoint.x = position.x();
00112     startPoint.y = position.y();
00113     startPoint.z = position.z();
00114     marker_.points.push_back(startPoint);
00115 
00116     geometry_msgs::Point endPoint;
00117     endPoint.x = startPoint.x + scale_ * vector.x;
00118     endPoint.y = startPoint.y + scale_ * vector.y;
00119     endPoint.z = startPoint.z + scale_ * vector.z;
00120     marker_.points.push_back(endPoint);
00121 
00122     marker_.colors.push_back(color_); // Each vertex needs a color.
00123     marker_.colors.push_back(color_);
00124   }
00125 
00126   publisher_.publish(marker_);
00127   return true;
00128 }
00129 
00130 } /* namespace */


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