visualization.hpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann
00037  *********************************************************************/
00038 
00039 #include <teb_local_planner/visualization.h>
00040 #include <boost/utility.hpp>
00041 
00042 
00043 namespace teb_local_planner
00044 {
00045  
00046 
00047 template <typename GraphType>
00048 void TebVisualization::publishGraph(const GraphType& graph, const std::string& ns_prefix)
00049 {        
00050   if ( printErrorWhenNotInitialized() )
00051     return;
00052   
00053   typedef typename boost::graph_traits<GraphType>::vertex_iterator GraphVertexIterator;
00054   typedef typename boost::graph_traits<GraphType>::edge_iterator GraphEdgeIterator;
00055 
00056   // Visualize Edges
00057   visualization_msgs::Marker marker;
00058   marker.header.frame_id = visualization_frame_;
00059   marker.header.stamp = ros::Time::now();
00060   marker.ns = ns_prefix + "Edges";
00061   marker.id = 0;
00062 // #define TRIANGLE
00063 #ifdef TRIANGLE
00064   marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00065 #else
00066   marker.type = visualization_msgs::Marker::LINE_LIST;
00067 #endif
00068   marker.action = visualization_msgs::Marker::ADD;
00069   
00070   GraphEdgeIterator it_edge, end_edges;
00071   for (boost::tie(it_edge,end_edges) = boost::edges(graph); it_edge!=end_edges; ++it_edge)
00072   {
00073 #ifdef TRIANGLE
00074     geometry_msgs::Point point_start1;
00075     point_start1.x = graph[boost::source(*it_edge,graph)].pos[0]+0.05;
00076     point_start1.y = graph[boost::source(*it_edge,graph)].pos[1]-0.05;
00077     point_start1.z = 0;
00078     marker.points.push_back(point_start1);
00079     geometry_msgs::Point point_start2;
00080     point_start2.x = graph[boost::source(*it_edge,graph)].pos[0]-0.05;
00081     point_start2.y = graph[boost::source(*it_edge,graph)].pos[1]+0.05;
00082     point_start2.z = 0;
00083     marker.points.push_back(point_start2);
00084 
00085 #else
00086     geometry_msgs::Point point_start;
00087     point_start.x = graph[boost::source(*it_edge,graph)].pos[0];
00088     point_start.y = graph[boost::source(*it_edge,graph)].pos[1];
00089     point_start.z = 0;
00090     marker.points.push_back(point_start);
00091 #endif
00092     geometry_msgs::Point point_end;
00093     point_end.x = graph[boost::target(*it_edge,graph)].pos[0];
00094     point_end.y = graph[boost::target(*it_edge,graph)].pos[1];
00095     point_end.z = 0;
00096     marker.points.push_back(point_end);
00097     
00098     // add color
00099     std_msgs::ColorRGBA color;
00100     color.a = 1.0;
00101     color.r = 0;
00102     color.g = 0;
00103     color.b = 1;
00104     marker.colors.push_back(color);
00105     marker.colors.push_back(color);
00106 #ifdef TRIANGLE
00107     marker.colors.push_back(color);
00108 #endif
00109   }
00110   
00111 #ifdef TRIANGLE
00112   marker.scale.x = 1;
00113   marker.scale.y = 1;
00114   marker.scale.z = 1;
00115 #else 
00116   marker.scale.x = 0.01;
00117 #endif
00118   marker.color.a = 1.0;
00119   marker.color.r = 0.0;
00120   marker.color.g = 1.0;
00121   marker.color.b = 0.0;
00122 
00123   // Now publish edge markers
00124   teb_marker_pub_.publish( marker );
00125   
00126   // Visualize vertices
00127   marker.header.frame_id = visualization_frame_;
00128   marker.header.stamp = ros::Time::now();
00129   marker.ns = ns_prefix + "Vertices";
00130   marker.id = 0;
00131   marker.type = visualization_msgs::Marker::POINTS;
00132   marker.action = visualization_msgs::Marker::ADD;
00133   
00134   GraphVertexIterator it_vert, end_vert;
00135   for (boost::tie(it_vert,end_vert) = boost::vertices(graph); it_vert!=end_vert; ++it_vert)
00136   {
00137     geometry_msgs::Point point;
00138     point.x = graph[*it_vert].pos[0];
00139     point.y = graph[*it_vert].pos[1];
00140     point.z = 0;
00141     marker.points.push_back(point);
00142     // add color
00143     
00144     std_msgs::ColorRGBA color;
00145     color.a = 1.0;
00146     if (it_vert==end_vert-1)
00147     {
00148       color.r = 1;
00149       color.g = 0;
00150       color.b = 0;              
00151     }
00152     else
00153     {
00154       color.r = 0;
00155       color.g = 1;
00156       color.b = 0;
00157     }
00158     marker.colors.push_back(color);
00159   }
00160   // set first color (start vertix) to blue
00161   if (!marker.colors.empty())
00162   {
00163     marker.colors.front().b = 1;
00164     marker.colors.front().g = 0;
00165   }
00166   
00167   marker.scale.x = 0.1;
00168   marker.scale.y = 0.1;
00169   marker.color.a = 1.0;
00170   marker.color.r = 0.0;
00171   marker.color.g = 1.0;
00172   marker.color.b = 0.0;
00173 
00174   // Now publish vertex markers
00175   teb_marker_pub_.publish( marker );
00176 }
00177   
00178 template <typename BidirIter>
00179 void TebVisualization::publishPathContainer(BidirIter first, BidirIter last, const std::string& ns)
00180 {
00181   if ( printErrorWhenNotInitialized() )
00182     return;
00183   
00184   visualization_msgs::Marker marker;
00185   marker.header.frame_id = visualization_frame_;
00186   marker.header.stamp = ros::Time::now();
00187   marker.ns = ns;
00188   marker.id = 0;
00189   marker.type = visualization_msgs::Marker::LINE_LIST;
00190   marker.action = visualization_msgs::Marker::ADD;
00191 
00192   typedef typename std::iterator_traits<BidirIter>::value_type PathType; // Get type of the path (point container)
00193   
00194   // Iterate through path container
00195   while(first != last)
00196   {       
00197     // iterate single path points
00198     typename PathType::const_iterator it_point, end_point;
00199     for (it_point = first->begin(), end_point = boost::prior(first->end()); it_point != end_point; ++it_point) 
00200     {
00201       geometry_msgs::Point point_start;
00202       point_start.x = get_const_reference(*it_point).x();
00203       point_start.y = get_const_reference(*it_point).y();
00204       point_start.z = 0;
00205       marker.points.push_back(point_start);
00206 
00207       geometry_msgs::Point point_end;
00208       point_end.x = get_const_reference(*boost::next(it_point)).x();
00209       point_end.y = get_const_reference(*boost::next(it_point)).y();
00210       point_end.z = 0;
00211       marker.points.push_back(point_end);
00212     }
00213     ++first;
00214   }
00215   marker.scale.x = 0.01;
00216   marker.color.a = 1.0;
00217   marker.color.r = 0.0;
00218   marker.color.g = 1.0;
00219   marker.color.b = 0.0;
00220 
00221   teb_marker_pub_.publish( marker );
00222 }
00223   
00224   
00225 } // namespace teb_local_planner


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Sat Jun 8 2019 20:21:34