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
26using 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
40void
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
52void
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
71void
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 */
176void
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}
virtual void loop()
Code to execute in the thread.
void publish(const NavGraphGeneratorThread::ObstacleMap &obstacles, const NavGraphGeneratorThread::ObstacleMap &map_obstacles, const NavGraphGeneratorThread::PoiMap &pois)
Trigger publishing of visualization.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
void wakeup()
Wake up thread.
Definition: thread.cpp:995
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:729
Fawkes library namespace.