rviz.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
35 #ifndef _RVIZ_CPP_
36 #define _RVIZ_CPP_
37 
38 #include "follow_pedestrian.h"
39 
40 void add_to_viz_markers_vec(std::vector<visualization_msgs::Marker>* marker_vec)
41 {
42  visualization_msgs::Marker marker;
43  geometry_msgs::Point p;
44  static ros::Time trigger_blink=ros::Time::now();
45  static bool flip=false;
46 
47  //Trigger zone cylinder
48  marker.header.frame_id = "/atc/vehicle/ground";
49  marker.header.stamp = ros::Time();
50  marker.ns = "trigger_zone";
51  marker.id = 0;
52  marker.type = visualization_msgs::Marker::CYLINDER;
53  marker.action = visualization_msgs::Marker::ADD;
54  marker.pose.position.x = search_area.x;
55  marker.pose.position.y = search_area.y;
56  marker.pose.position.z = 0;
57  marker.pose.orientation.x = 0.0;
58  marker.pose.orientation.y = 0.0;
59  marker.pose.orientation.z = 0.0;
60  marker.pose.orientation.w = 1.0;
61  marker.scale.x = 1;
62  marker.scale.y = 1;
63  marker.scale.z = 0.1;
64  marker.color.a = 1.0;
65 
66  if ((ros::Time::now()-trigger_blink).toSec() > 1)
67  {
68  flip=!flip;
69  trigger_blink = ros::Time::now();
70  }
71 
72  if (flip)
73  {
74  marker.color.r = 0.5;
75  marker.color.g = 0.5;
76  marker.color.b = 0.0;
77  }
78  else
79  {
80  marker.color.r = 1;
81  marker.color.g = 1;
82  marker.color.b = 0.0;
83  }
84  marker_vec->push_back(marker);
85 
86  //Trigger zone Text
87  marker.header.frame_id = "/atc/vehicle/ground";
88  marker.header.stamp = ros::Time();
89  marker.ns = "trigger_text";
90  marker.id = 0;
91  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
92  marker.action = visualization_msgs::Marker::ADD;
93  marker.pose.position.x = search_area.x;
94  marker.pose.position.y = search_area.y;
95  marker.pose.position.z = 0.3;
96  marker.pose.orientation.x = 0.0;
97  marker.pose.orientation.y = 0.0;
98  marker.pose.orientation.z = 0.0;
99  marker.pose.orientation.w = 1.0;
100  marker.scale.x = search_area.radius;
101  marker.scale.y = search_area.radius;
102  marker.scale.z = 0.3;
103  marker.color.a = 1.0;
104  marker.color.r = 0.0;
105  marker.color.g = 0.0;
106  marker.color.b = 0.0;
107  if (status.state==SEARCHING)
108  marker.text = "SEARCHING: Waiting for someone";
109  else if (status.state==TRACKING)
110  {
111  char str[1024];
112  sprintf(str,"TRACKING: Following target id %d",status.target_id);
113  marker.text = str;
114  }
115  else if (status.state==TRACKING_NOT_SAFE)
116  {
117  char str[1024];
118  sprintf(str,"TRACKING_NOT_SAFE: Waiting for clearance to follow target id %d",status.target_id);
119  marker.text = str;
120  }
121 
122  marker_vec->push_back(marker);
123 
124  //Safety zone lines
125  marker.header.frame_id = "/atc/vehicle/ground";
126  marker.header.stamp = ros::Time();
127  marker.ns = "safety_zone";
128  marker.id = 0;
129  marker.type = visualization_msgs::Marker::LINE_STRIP;
130  marker.action = visualization_msgs::Marker::ADD;
131  marker.pose.position.x = 0;
132  marker.pose.position.y = 0;
133  marker.pose.position.z = 0;
134  marker.pose.orientation.x = 0.0;
135  marker.pose.orientation.y = 0.0;
136  marker.pose.orientation.z = 0.0;
137  marker.pose.orientation.w = 1.0;
138  marker.scale.x = 0.2;
139  marker.scale.y = 1;
140  marker.scale.z = 0.2;
141  marker.color.a = 1.0;
142 
143  if (!is_safe_using_lasers())//replace here by the danger flag
144  {
145  marker.color.r = 1;
146  marker.color.g = 0;
147  marker.color.b = 0;
148  }
149  else
150  {
151  marker.color.r = 0;
152  marker.color.g = 1;
153  marker.color.b = 0;
154  }
155 
156  p.x = safety_zone.xmin; p.y = safety_zone.ymin; p.z = 0;
157  marker.points.push_back(p);
158 
159  p.x = safety_zone.xmin; p.y = safety_zone.ymax; p.z = 0;
160  marker.points.push_back(p);
161  marker.points.push_back(p);
162 
163  p.x = safety_zone.xmax; p.y = safety_zone.ymax; p.z = 0;
164  marker.points.push_back(p);
165  marker.points.push_back(p);
166 
167  p.x = safety_zone.xmax; p.y = safety_zone.ymin; p.z = 0;
168  marker.points.push_back(p);
169  marker.points.push_back(p);
170 
171  p.x = safety_zone.xmin; p.y = safety_zone.ymin; p.z = 0;
172  marker.points.push_back(p);
173 
174  marker_vec->push_back(marker);
175 
176  //send the obj model for the pedestrian marker
177  marker.id = 1;
178  marker.header.stamp = ros::Time::now();
179  marker.type = visualization_msgs::Marker::MESH_RESOURCE;
180  marker.action = visualization_msgs::Marker::ADD;
181  marker.ns = "pedestrian";
182  marker.mesh_use_embedded_materials = 1;
183  marker.header.frame_id = "/atc/vehicle/ground";
184  marker.scale.x = .4;
185  marker.scale.y = .4;
186  marker.scale.z = .4;
187  marker.color.r = 0.2;
188  marker.color.g = 0.3;
189  marker.color.b = 0.4;
190  marker.color.a = 1;
191  marker.mesh_resource = "package://wrapper_collada/models/decisive_woman.obj";
192  marker.header.stamp = ros::Time::now();
193 
194  if (status.state==TRACKING || status.state == TRACKING_NOT_SAFE)
195  {
196  marker.pose.position.x = status.current_x;
197  marker.pose.position.y = status.current_y;
198  marker.pose.position.z = status.current_z;
199  marker.pose.orientation = status.current_q;
200  }
201  else
202  {
203  marker.pose.position.x = 0;
204  marker.pose.position.y = 0;
205  marker.pose.position.z = 3000;
206  }
207 
208  marker_vec->push_back(marker);
209 }
210 
211 
212 #endif
add_to_viz_markers_vec
void add_to_viz_markers_vec(std::vector< visualization_msgs::Marker > *marker_vec)
Definition: rviz.cpp:40
follow_pedestrian.h
Planar scan generator.

follow_pedestrian
Author(s): Miguel Oliveira, Jorge Almeida
autogenerated on Mon Mar 2 2015 01:31:38