HectorDrawings.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef HECTOR_DRAWINGS_H__
00030 #define HECTOR_DRAWINGS_H__
00031 
00032 #include "util/DrawInterface.h"
00033 #include "util/UtilFunctions.h"
00034 
00035 #include "ros/ros.h"
00036 
00037 #include <visualization_msgs/MarkerArray.h>
00038 
00039 #include <Eigen/Dense>
00040 
00041 
00042 class HectorDrawings : public DrawInterface
00043 {
00044 public:
00045 
00046   HectorDrawings()
00047   {
00048     idCounter = 0;
00049 
00050     ros::NodeHandle nh_;
00051 
00052     markerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 1, true);
00053     markerArrayPublisher_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1, true);
00054 
00055     tempMarker.header.frame_id = "map";
00056     tempMarker.ns = "slam";
00057 
00058     this->setScale(1.0);
00059     this->setColor(1.0, 1.0, 1.0);
00060 
00061     tempMarker.action = visualization_msgs::Marker::ADD;
00062   };
00063 
00064   virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame)
00065   {
00066     tempMarker.id = idCounter++;
00067 
00068     tempMarker.pose.position.x = pointWorldFrame.x();
00069     tempMarker.pose.position.y = pointWorldFrame.y();
00070 
00071     tempMarker.pose.orientation.w = 0.0;
00072     tempMarker.pose.orientation.z = 0.0;
00073     tempMarker.type = visualization_msgs::Marker::CUBE;
00074 
00075     //markerPublisher_.publish(tempMarker);
00076 
00077     markerArray.markers.push_back(tempMarker);
00078   }
00079 
00080   virtual void drawArrow(const Eigen::Vector3f& poseWorld)
00081   {
00082     tempMarker.id = idCounter++;
00083 
00084     tempMarker.pose.position.x = poseWorld.x();
00085     tempMarker.pose.position.y = poseWorld.y();
00086 
00087     tempMarker.pose.orientation.w = cos(poseWorld.z()*0.5f);
00088     tempMarker.pose.orientation.z = sin(poseWorld.z()*0.5f);
00089 
00090     tempMarker.type = visualization_msgs::Marker::ARROW;
00091 
00092     //markerPublisher_.publish(tempMarker);
00093 
00094     markerArray.markers.push_back(tempMarker);
00095 
00096   }
00097 
00098   virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& covMatrix)
00099   {
00100     tempMarker.pose.position.x = mean[0];
00101     tempMarker.pose.position.y = mean[1];
00102 
00103     Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(covMatrix);
00104 
00105     const Eigen::Vector2f& eigValues (eig.eigenvalues());
00106     const Eigen::Matrix2f& eigVectors (eig.eigenvectors());
00107 
00108     float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0)));
00109 
00110     tempMarker.type = visualization_msgs::Marker::CYLINDER;
00111 
00112     double lengthMajor = sqrt(eigValues[0]);
00113     double lengthMinor = sqrt(eigValues[1]);
00114 
00115     tempMarker.scale.x = lengthMajor;
00116     tempMarker.scale.y = lengthMinor;
00117     tempMarker.scale.z = 0.001;
00118 
00119 
00120     tempMarker.pose.orientation.w = cos(angle*0.5);
00121     tempMarker.pose.orientation.z = sin(angle*0.5);
00122 
00123     tempMarker.id = idCounter++;
00124     markerArray.markers.push_back(tempMarker);
00125 
00126     //drawLine(Eigen::Vector3f(0,0,0), Eigen::Vector3f(lengthMajor ,0,0));
00127     //drawLine(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0,lengthMinor,0));
00128 
00129     //glScalef(lengthMajor, lengthMinor, 0);
00130     //glCallList(dlCircle);
00131     //this->popCS();
00132   }
00133 
00134   virtual void setScale(double scale)
00135   {
00136     tempMarker.scale.x = scale;
00137     tempMarker.scale.y = scale;
00138     tempMarker.scale.z = scale;
00139   }
00140 
00141   virtual void setColor(double r, double g, double b, double a = 1.0)
00142   {
00143     tempMarker.color.r = r;
00144     tempMarker.color.g = g;
00145     tempMarker.color.b = b;
00146     tempMarker.color.a = a;
00147   }
00148 
00149   virtual void sendAndResetData()
00150   {
00151     markerArrayPublisher_.publish(markerArray);
00152     markerArray.markers.clear();
00153     idCounter = 0;
00154   }
00155 
00156   void setTime(const ros::Time& time)
00157   {
00158     tempMarker.header.stamp = time;
00159   }
00160 
00161 
00162   ros::Publisher markerPublisher_;
00163   ros::Publisher markerArrayPublisher_;
00164 
00165   visualization_msgs::Marker tempMarker;
00166   visualization_msgs::MarkerArray markerArray;
00167 
00168   int idCounter;
00169 };
00170 
00171 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Wed Aug 26 2015 11:45:03