Fawkes API  Fawkes Development Version
visualization_thread.cpp
1 
2 /***************************************************************************
3  * visualization_thread.cpp - Visualization for navgraph-generator via rviz
4  *
5  * Created: Fri Mar 27 12:12:17 2015
6  * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "visualization_thread.h"
23 
24 #include <ros/ros.h>
25 
26 using namespace fawkes;
27 
28 /** @class NavGraphGeneratorVisualizationThread "visualization_thread.h"
29  * Send Marker messages to rviz to show navgraph-generator info.
30  * @author Tim Niemueller
31  */
32 
33 /** Constructor. */
35 : fawkes::Thread("NavGraphGeneratorVisualizationThread", Thread::OPMODE_WAITFORWAKEUP)
36 {
38 }
39 
40 void
42 {
43  cfg_global_frame_ = config->get_string("/frames/fixed");
44 
45  vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>("visualization_marker_array",
46  100,
47  /* latching */ true);
48 
49  last_id_num_ = 0;
50 }
51 
52 void
54 {
55  visualization_msgs::MarkerArray m;
56 
57  for (size_t i = 0; i < last_id_num_; ++i) {
58  visualization_msgs::Marker delop;
59  delop.header.frame_id = "/map";
60  delop.header.stamp = ros::Time::now();
61  delop.ns = "navgraph_generator";
62  delop.id = i;
63  delop.action = visualization_msgs::Marker::DELETE;
64  m.markers.push_back(delop);
65  }
66  vispub_.publish(m);
67  usleep(500000); // needs some time to actually send
68  vispub_.shutdown();
69 }
70 
71 void
73 {
74  visualization_msgs::MarkerArray m;
75  unsigned int idnum = 0;
76 
77  for (auto &o : obstacles_) {
78  visualization_msgs::Marker text;
79  text.header.frame_id = cfg_global_frame_;
80  text.header.stamp = ros::Time::now();
81  text.ns = "navgraph_generator";
82  text.id = idnum++;
83  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
84  text.action = visualization_msgs::Marker::ADD;
85  text.pose.position.x = o.second.x;
86  text.pose.position.y = o.second.y;
87  text.pose.position.z = .15;
88  text.pose.orientation.w = 1.;
89  text.scale.z = 0.15;
90  text.color.r = text.color.g = text.color.b = 1.0f;
91  text.color.a = 1.0;
92  text.lifetime = ros::Duration(0, 0);
93  text.text = o.first;
94  m.markers.push_back(text);
95 
96  visualization_msgs::Marker sphere;
97  sphere.header.frame_id = cfg_global_frame_;
98  sphere.header.stamp = ros::Time::now();
99  sphere.ns = "navgraph_generator";
100  sphere.id = idnum++;
101  sphere.type = visualization_msgs::Marker::SPHERE;
102  sphere.action = visualization_msgs::Marker::ADD;
103  sphere.pose.position.x = o.second.x;
104  sphere.pose.position.y = o.second.y;
105  sphere.pose.position.z = 0.05;
106  sphere.pose.orientation.w = 1.;
107  sphere.scale.x = 0.05;
108  sphere.scale.y = 0.05;
109  sphere.scale.z = 0.05;
110  sphere.color.r = 1.0;
111  sphere.color.g = sphere.color.b = 0.;
112  sphere.color.a = 1.0;
113  sphere.lifetime = ros::Duration(0, 0);
114  m.markers.push_back(sphere);
115  }
116 
117  for (auto &o : map_obstacles_) {
118  visualization_msgs::Marker text;
119  text.header.frame_id = cfg_global_frame_;
120  text.header.stamp = ros::Time::now();
121  text.ns = "navgraph_generator";
122  text.id = idnum++;
123  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
124  text.action = visualization_msgs::Marker::ADD;
125  text.pose.position.x = o.second.x;
126  text.pose.position.y = o.second.y;
127  text.pose.position.z = .15;
128  text.pose.orientation.w = 1.;
129  text.scale.z = 0.15;
130  text.color.r = text.color.g = text.color.b = 1.0f;
131  text.color.a = 1.0;
132  text.lifetime = ros::Duration(0, 0);
133  text.text = o.first;
134  m.markers.push_back(text);
135 
136  visualization_msgs::Marker sphere;
137  sphere.header.frame_id = cfg_global_frame_;
138  sphere.header.stamp = ros::Time::now();
139  sphere.ns = "navgraph_generator";
140  sphere.id = idnum++;
141  sphere.type = visualization_msgs::Marker::SPHERE;
142  sphere.action = visualization_msgs::Marker::ADD;
143  sphere.pose.position.x = o.second.x;
144  sphere.pose.position.y = o.second.y;
145  sphere.pose.position.z = 0.05;
146  sphere.pose.orientation.w = 1.;
147  sphere.scale.x = 0.05;
148  sphere.scale.y = 0.05;
149  sphere.scale.z = 0.05;
150  sphere.color.r = sphere.color.g = 1.0;
151  sphere.color.b = 0.;
152  sphere.color.a = 1.0;
153  sphere.lifetime = ros::Duration(0, 0);
154  m.markers.push_back(sphere);
155  }
156 
157  for (size_t i = idnum; i < last_id_num_; ++i) {
158  visualization_msgs::Marker delop;
159  delop.header.frame_id = cfg_global_frame_;
160  delop.header.stamp = ros::Time::now();
161  delop.ns = "navgraph_generator";
162  delop.id = i;
163  delop.action = visualization_msgs::Marker::DELETE;
164  m.markers.push_back(delop);
165  }
166  last_id_num_ = idnum;
167 
168  vispub_.publish(m);
169 }
170 
171 /** Trigger publishing of visualization.
172  * @param obstacles obstacles used for graph generation
173  * @param map_obstacles obstacles generated from map
174  * @param pois points of interest
175  */
176 void
178  const NavGraphGeneratorThread::ObstacleMap &obstacles,
179  const NavGraphGeneratorThread::ObstacleMap &map_obstacles,
180  const NavGraphGeneratorThread::PoiMap & pois)
181 {
182  obstacles_ = obstacles;
183  map_obstacles_ = map_obstacles;
184  pois_ = pois;
185 
186  wakeup();
187 }
NavGraphGeneratorVisualizationThread::finalize
virtual void finalize()
Finalize the thread.
Definition: visualization_thread.cpp:53
fawkes::Thread::wakeup
void wakeup()
Wake up thread.
Definition: thread.cpp:999
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Definition: ros.h:46
NavGraphGeneratorVisualizationThread::NavGraphGeneratorVisualizationThread
NavGraphGeneratorVisualizationThread()
Constructor.
Definition: visualization_thread.cpp:34
fawkes::Thread::set_coalesce_wakeups
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:733
NavGraphGeneratorVisualizationThread::loop
virtual void loop()
Code to execute in the thread.
Definition: visualization_thread.cpp:72
fawkes
NavGraphGeneratorVisualizationThread::publish
void publish(const NavGraphGeneratorThread::ObstacleMap &obstacles, const NavGraphGeneratorThread::ObstacleMap &map_obstacles, const NavGraphGeneratorThread::PoiMap &pois)
Trigger publishing of visualization.
Definition: visualization_thread.cpp:177
NavGraphGeneratorVisualizationThread::init
virtual void init()
Initialize the thread.
Definition: visualization_thread.cpp:41
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:50
fawkes::Thread
Definition: thread.h:44
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0