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 
41 void
43 {
44  cfg_global_frame_ = config->get_string("/frames/fixed");
45 
46  vispub_ =
47  rosnode->advertise<visualization_msgs::MarkerArray>
48  ("visualization_marker_array", 100, /* latching */ true);
49 
50  last_id_num_ = 0;
51 }
52 
53 void
55 {
56  visualization_msgs::MarkerArray m;
57 
58  for (size_t i = 0; i < last_id_num_; ++i) {
59  visualization_msgs::Marker delop;
60  delop.header.frame_id = "/map";
61  delop.header.stamp = ros::Time::now();
62  delop.ns = "navgraph_generator";
63  delop.id = i;
64  delop.action = visualization_msgs::Marker::DELETE;
65  m.markers.push_back(delop);
66  }
67  vispub_.publish(m);
68  usleep(500000); // needs some time to actually send
69  vispub_.shutdown();
70 }
71 
72 
73 void
75 {
76  visualization_msgs::MarkerArray m;
77  unsigned int idnum = 0;
78 
79  for (auto &o : obstacles_) {
80  visualization_msgs::Marker text;
81  text.header.frame_id = cfg_global_frame_;
82  text.header.stamp = ros::Time::now();
83  text.ns = "navgraph_generator";
84  text.id = idnum++;
85  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
86  text.action = visualization_msgs::Marker::ADD;
87  text.pose.position.x = o.second.x;
88  text.pose.position.y = o.second.y;
89  text.pose.position.z = .15;
90  text.pose.orientation.w = 1.;
91  text.scale.z = 0.15;
92  text.color.r = text.color.g = text.color.b = 1.0f;
93  text.color.a = 1.0;
94  text.lifetime = ros::Duration(0, 0);
95  text.text = o.first;
96  m.markers.push_back(text);
97 
98  visualization_msgs::Marker sphere;
99  sphere.header.frame_id = cfg_global_frame_;
100  sphere.header.stamp = ros::Time::now();
101  sphere.ns = "navgraph_generator";
102  sphere.id = idnum++;
103  sphere.type = visualization_msgs::Marker::SPHERE;
104  sphere.action = visualization_msgs::Marker::ADD;
105  sphere.pose.position.x = o.second.x;
106  sphere.pose.position.y = o.second.y;
107  sphere.pose.position.z = 0.05;
108  sphere.pose.orientation.w = 1.;
109  sphere.scale.x = 0.05;
110  sphere.scale.y = 0.05;
111  sphere.scale.z = 0.05;
112  sphere.color.r = 1.0;
113  sphere.color.g = sphere.color.b = 0.;
114  sphere.color.a = 1.0;
115  sphere.lifetime = ros::Duration(0, 0);
116  m.markers.push_back(sphere);
117  }
118 
119  for (auto &o : map_obstacles_) {
120  visualization_msgs::Marker text;
121  text.header.frame_id = cfg_global_frame_;
122  text.header.stamp = ros::Time::now();
123  text.ns = "navgraph_generator";
124  text.id = idnum++;
125  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
126  text.action = visualization_msgs::Marker::ADD;
127  text.pose.position.x = o.second.x;
128  text.pose.position.y = o.second.y;
129  text.pose.position.z = .15;
130  text.pose.orientation.w = 1.;
131  text.scale.z = 0.15;
132  text.color.r = text.color.g = text.color.b = 1.0f;
133  text.color.a = 1.0;
134  text.lifetime = ros::Duration(0, 0);
135  text.text = o.first;
136  m.markers.push_back(text);
137 
138  visualization_msgs::Marker sphere;
139  sphere.header.frame_id = cfg_global_frame_;
140  sphere.header.stamp = ros::Time::now();
141  sphere.ns = "navgraph_generator";
142  sphere.id = idnum++;
143  sphere.type = visualization_msgs::Marker::SPHERE;
144  sphere.action = visualization_msgs::Marker::ADD;
145  sphere.pose.position.x = o.second.x;
146  sphere.pose.position.y = o.second.y;
147  sphere.pose.position.z = 0.05;
148  sphere.pose.orientation.w = 1.;
149  sphere.scale.x = 0.05;
150  sphere.scale.y = 0.05;
151  sphere.scale.z = 0.05;
152  sphere.color.r = sphere.color.g = 1.0;
153  sphere.color.b = 0.;
154  sphere.color.a = 1.0;
155  sphere.lifetime = ros::Duration(0, 0);
156  m.markers.push_back(sphere);
157  }
158 
159  for (size_t i = idnum; i < last_id_num_; ++i) {
160  visualization_msgs::Marker delop;
161  delop.header.frame_id = cfg_global_frame_;
162  delop.header.stamp = ros::Time::now();
163  delop.ns = "navgraph_generator";
164  delop.id = i;
165  delop.action = visualization_msgs::Marker::DELETE;
166  m.markers.push_back(delop);
167  }
168  last_id_num_ = idnum;
169 
170  vispub_.publish(m);
171 }
172 
173 
174 /** Trigger publishing of visualization.
175  * @param obstacles obstacles used for graph generation
176  * @param map_obstacles obstacles generated from map
177  * @param pois points of interest
178  */
179 void
180 NavGraphGeneratorVisualizationThread::publish(const NavGraphGeneratorThread::ObstacleMap &obstacles,
181  const NavGraphGeneratorThread::ObstacleMap &map_obstacles,
182  const NavGraphGeneratorThread::PoiMap &pois)
183 {
184  obstacles_ = obstacles;
185  map_obstacles_ = map_obstacles;
186  pois_ = pois;
187 
188  wakeup();
189 }
Fawkes library namespace.
void publish(const NavGraphGeneratorThread::ObstacleMap &obstacles, const NavGraphGeneratorThread::ObstacleMap &map_obstacles, const NavGraphGeneratorThread::PoiMap &pois)
Trigger publishing of visualization.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void loop()
Code to execute in the thread.
void wakeup()
Wake up thread.
Definition: thread.cpp:1000
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:741
virtual void init()
Initialize the thread.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual void finalize()
Finalize the thread.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.