follow_pedestrian: /home/laradmin/lar/demos/follow_pedestrian/src/rviz.cpp Source File

00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00035 #ifndef _RVIZ_CPP_
00036 #define _RVIZ_CPP_
00037 
00038 #include "follow_pedestrian.h"
00039 
00040 void add_to_viz_markers_vec(std::vector<visualization_msgs::Marker>* marker_vec)
00041 {
00042         visualization_msgs::Marker marker;
00043         geometry_msgs::Point p;
00044         static ros::Time trigger_blink=ros::Time::now();
00045         static bool flip=false;
00046 
00047         //Trigger zone cylinder
00048         marker.header.frame_id = "/atc/vehicle/ground";
00049         marker.header.stamp = ros::Time();
00050         marker.ns = "trigger_zone";
00051         marker.id = 0;
00052         marker.type = visualization_msgs::Marker::CYLINDER;
00053         marker.action = visualization_msgs::Marker::ADD;
00054         marker.pose.position.x = search_area.x;
00055         marker.pose.position.y = search_area.y;
00056         marker.pose.position.z = 0;
00057         marker.pose.orientation.x = 0.0;
00058         marker.pose.orientation.y = 0.0;
00059         marker.pose.orientation.z = 0.0;
00060         marker.pose.orientation.w = 1.0;
00061         marker.scale.x = 1;
00062         marker.scale.y = 1;
00063         marker.scale.z = 0.1;
00064         marker.color.a = 1.0;
00065 
00066         if ((ros::Time::now()-trigger_blink).toSec() > 1)
00067         {
00068                 flip=!flip;
00069                 trigger_blink = ros::Time::now();
00070         }
00071 
00072         if (flip)
00073         {
00074                 marker.color.r = 0.5;
00075                 marker.color.g = 0.5;
00076                 marker.color.b = 0.0;
00077         }
00078         else
00079         {
00080                 marker.color.r = 1;
00081                 marker.color.g = 1;
00082                 marker.color.b = 0.0;
00083         }
00084         marker_vec->push_back(marker);
00085 
00086         //Trigger zone Text
00087         marker.header.frame_id = "/atc/vehicle/ground";
00088         marker.header.stamp = ros::Time();
00089         marker.ns = "trigger_text";
00090         marker.id = 0;
00091         marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00092         marker.action = visualization_msgs::Marker::ADD;
00093         marker.pose.position.x = search_area.x;
00094         marker.pose.position.y = search_area.y;
00095         marker.pose.position.z = 0.3;
00096         marker.pose.orientation.x = 0.0;
00097         marker.pose.orientation.y = 0.0;
00098         marker.pose.orientation.z = 0.0;
00099         marker.pose.orientation.w = 1.0;
00100         marker.scale.x = search_area.radius;
00101         marker.scale.y = search_area.radius;
00102         marker.scale.z = 0.3;
00103         marker.color.a = 1.0;
00104         marker.color.r = 0.0;
00105         marker.color.g = 0.0;
00106         marker.color.b = 0.0;
00107         if (status.state==SEARCHING)
00108                 marker.text = "SEARCHING: Waiting for someone";
00109         else if (status.state==TRACKING)
00110         {
00111                 char str[1024];
00112                 sprintf(str,"TRACKING: Following target id %d",status.target_id);
00113                 marker.text = str;
00114         }
00115         else if (status.state==TRACKING_NOT_SAFE)
00116         {
00117                 char str[1024];
00118                 sprintf(str,"TRACKING_NOT_SAFE: Waiting for clearance to follow target id %d",status.target_id);
00119                 marker.text = str;
00120         }
00121 
00122         marker_vec->push_back(marker);
00123 
00124         //Safety zone lines
00125         marker.header.frame_id = "/atc/vehicle/ground";
00126         marker.header.stamp = ros::Time();
00127         marker.ns = "safety_zone";
00128         marker.id = 0;
00129         marker.type = visualization_msgs::Marker::LINE_STRIP;
00130         marker.action = visualization_msgs::Marker::ADD;
00131         marker.pose.position.x = 0;
00132         marker.pose.position.y = 0;
00133         marker.pose.position.z = 0;
00134         marker.pose.orientation.x = 0.0;
00135         marker.pose.orientation.y = 0.0;
00136         marker.pose.orientation.z = 0.0;
00137         marker.pose.orientation.w = 1.0;
00138         marker.scale.x = 0.2;
00139         marker.scale.y = 1;
00140         marker.scale.z = 0.2;
00141         marker.color.a = 1.0;
00142 
00143         if (!is_safe_using_lasers())//replace here by the danger flag
00144         {
00145                 marker.color.r = 1;
00146                 marker.color.g = 0;
00147                 marker.color.b = 0;
00148         }
00149         else
00150         {
00151                 marker.color.r = 0;
00152                 marker.color.g = 1;
00153                 marker.color.b = 0;
00154         }
00155 
00156         p.x = safety_zone.xmin; p.y = safety_zone.ymin; p.z = 0;
00157         marker.points.push_back(p);
00158 
00159         p.x = safety_zone.xmin; p.y = safety_zone.ymax; p.z = 0;
00160         marker.points.push_back(p);
00161         marker.points.push_back(p);
00162 
00163         p.x = safety_zone.xmax; p.y = safety_zone.ymax; p.z = 0;
00164         marker.points.push_back(p);
00165         marker.points.push_back(p);
00166 
00167         p.x = safety_zone.xmax; p.y = safety_zone.ymin; p.z = 0;
00168         marker.points.push_back(p);
00169         marker.points.push_back(p);
00170 
00171         p.x = safety_zone.xmin; p.y = safety_zone.ymin; p.z = 0;
00172         marker.points.push_back(p);
00173 
00174         marker_vec->push_back(marker);
00175 
00176         //send the obj model for the pedestrian marker
00177         marker.id = 1;  
00178         marker.header.stamp = ros::Time::now(); 
00179         marker.type = visualization_msgs::Marker::MESH_RESOURCE;        
00180         marker.action = visualization_msgs::Marker::ADD;        
00181         marker.ns = "pedestrian";
00182         marker.mesh_use_embedded_materials = 1; 
00183         marker.header.frame_id = "/atc/vehicle/ground";
00184         marker.scale.x = .4;
00185         marker.scale.y = .4;
00186         marker.scale.z = .4;
00187         marker.color.r = 0.2;
00188         marker.color.g = 0.3;
00189         marker.color.b = 0.4;
00190         marker.color.a = 1;
00191         marker.mesh_resource = "package://wrapper_collada/models/decisive_woman.obj";
00192         marker.header.stamp = ros::Time::now(); 
00193 
00194         if (status.state==TRACKING || status.state == TRACKING_NOT_SAFE)
00195         {
00196                 marker.pose.position.x = status.current_x;
00197                 marker.pose.position.y = status.current_y;
00198                 marker.pose.position.z = status.current_z;
00199                 marker.pose.orientation = status.current_q;
00200         }
00201         else
00202         {
00203                 marker.pose.position.x = 0;
00204                 marker.pose.position.y = 0;
00205                 marker.pose.position.z = 3000;
00206         }
00207 
00208         marker_vec->push_back(marker);
00209 }
00210 
00211 
00212 #endif