00001 /* 00002 * Software License Agreement (BSD License) 00003 *Copyright (c) 2015, micROS Team 00004 http://micros.nudt.edu.cn/ 00005 *All rights reserved. 00006 * Copyright (c) 2009, Willow Garage, Inc. 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 Willow Garage, Inc. 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 //-------------------------------------------- 00037 /* 00038 00039 plan for RViz v1.0 00040 00041 00042 */ 00043 //-------------------------------------------- 00044 00045 #include "ros/ros.h" 00046 #include <visualization_msgs/Marker.h> 00047 00048 #define max(x,y) (x>y?x:y) 00049 00050 //parameter for the map reducing ratio 00051 extern float AltitudeZoom; 00052 00053 //-------------------------------------------- 00054 extern ros::Publisher Marker_Publisher; 00055 extern visualization_msgs::Marker Tail; //path 00056 extern visualization_msgs::Marker Terrain; //terrian（ID=999） 00057 extern visualization_msgs::Marker TerrainLine; //terrianline（ID=998） 00058 //-------------------------------------------- 00059 00060 //initialize the publisher 00061 void InitMarkerPublisher(); 00062 //chooss the color based on the ID 00063 void GetStoreColor(int ID,float& r,float& g,float& b); 00064 //intialize the marker 00065 void UInitMarker(visualization_msgs::Marker& marker); 00066 00067 //draw the cylinder 00068 void Draw_Cylinder(int ID,float x,float y,float z,float Rad,float Height,float r=1.0f,float g=1.0f,float b=1.0f,float a=1.0f); 00069 //draw the ball 00070 void Draw_Sphere(int ID,float x,float y,float z,float Rad,float r=1.0f,float g=1.0f,float b=1.0f,float a=1.0f); 00071 00072 //initialize the path 00073 void InitTail(int ID,float r=1.0f,float g=1.0f,float b=1.0f,float a=1.0f); 00074 //update the path 00075 void AddTailPoint(float x,float y,float z); 00076 //clean the path 00077 void ResetTail(); 00078 00079 //draw the terrian 00080 void DrawTerrain(int Width,int Height,int space,short* TerrainMap);